diff --git a/DiffWheel/DiffWheelC/AGV.cs b/DiffWheel/DiffWheelC/AGV.cs new file mode 100644 index 0000000..3d69570 --- /dev/null +++ b/DiffWheel/DiffWheelC/AGV.cs @@ -0,0 +1,18 @@ +using System; +using MDCSToolBox.Clumsy.AgvInterfaces; +using MDCSToolBox.Clumsy.MotionControllers; + +namespace DiffWheelC; + +public class AGV : DiffWheelInterface +{ + public override AbstractGeometricController GetController() + { + return new ChassisController().Get(); + } + + public override NaiveMagnetController GetNaiveMagnetController() + { + throw new NotImplementedException("Tutorial DiffWheelC does not implement magnetic tracking."); + } +} diff --git a/DiffWheel/DiffWheelC/ChassisController.cs b/DiffWheel/DiffWheelC/ChassisController.cs new file mode 100644 index 0000000..fdbb039 --- /dev/null +++ b/DiffWheel/DiffWheelC/ChassisController.cs @@ -0,0 +1,40 @@ +using ClumsyCore; +using ClumsyCore.Pilot; +using MDCSToolBox.Clumsy.MotionControllers; +using MDCSToolBox.Clumsy.Pilot; + +namespace DiffWheelC; + +public class ChassisController : MovementDefinition +{ + public float BaseSpeed = Configuration.conf.basicSpeed; + + public override DiffWheelGeometricController Get() + { + return new DiffWheelGeometricController + { + Chassis = BasicPilotBase.Chassis, + BaseSpeed = BaseSpeed, + SlowDistance = PilotDefinition.Conf.SlowDistance, + SlowingPow = PilotDefinition.Conf.SlowingPow, + FinishDistance = PilotDefinition.Conf.FinishDistance, + FinishSpeed = PilotDefinition.Conf.FinishSpeed, + FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy, + NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle, + FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac, + FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed, + DebugMode = PilotDefinition.Conf.MotionDebugPrint, + DebugCurvature = PilotDefinition.Conf.DebugCurvature, + MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit, + MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit, + SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin, + SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin, + SpeedAccPerSecond = PilotDefinition.Conf.SpeedAccPerSecond, + SpeedDeAccPerSecond = PilotDefinition.Conf.SpeedDeAccPerSecond, + PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead, + SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead, + SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff, + SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff, + }; + } +} diff --git a/DiffWheel/DiffWheelC/DiffWheelC.csproj b/DiffWheel/DiffWheelC/DiffWheelC.csproj new file mode 100644 index 0000000..31576ed --- /dev/null +++ b/DiffWheel/DiffWheelC/DiffWheelC.csproj @@ -0,0 +1,45 @@ + + + + netstandard2.0 + 10 + true + false + ..\..\build\Clumsy_diff\ + + + + + + + + + + D:\MDCS\Release\Clumsy\RefClumsyCore.dll + false + + + D:\MDCS\Release\Clumsy\RefClumsyDance.dll + false + + + D:\MDCS\Release\CommonUsage.dll + + + D:\MDCS\Release\deps\RefFundamentalLib.dll + false + + + D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll + false + + + D:\MDCS\Release\MDCSToolBox.dll + + + + + + + + diff --git a/DiffWheel/DiffWheelC/MovementTests.cs b/DiffWheel/DiffWheelC/MovementTests.cs new file mode 100644 index 0000000..e68a151 --- /dev/null +++ b/DiffWheel/DiffWheelC/MovementTests.cs @@ -0,0 +1,76 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using ClumsyCore; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MDCSToolBox.Clumsy.Movements; +using MDCSToolBox.Clumsy.Pilot; + +namespace DiffWheelC; + +public class DiffForwardTest : MovementDefinition +{ + public float Speed = 0.2f; + public float DurationSeconds = 2f; + + public override IEnumerable Get() + { + var chassis = (DifferentialChassis)BasicPilotBase.Chassis; + var end = DateTime.Now.AddSeconds(DurationSeconds); + while (DateTime.Now < end) + { + chassis.SendSpeed(Speed, 0); + yield return true; + } + + chassis.SendSpeed(0, 0); + } +} + +public class DiffRotateTest : MovementDefinition +{ + public float AngularSpeed = 30f; + public float DurationSeconds = 2f; + + public override IEnumerable Get() + { + var chassis = (DifferentialChassis)BasicPilotBase.Chassis; + var end = DateTime.Now.AddSeconds(DurationSeconds); + while (DateTime.Now < end) + { + chassis.SendSpeed(0, AngularSpeed); + yield return true; + } + + chassis.SendSpeed(0, 0); + } +} + +[MovementTest(name = "差速-前进2秒")] +public class DiffForwardMovementTest : MovementTest +{ + private DriveTask _task; + + public override void Test() + { + _task = new DriveTask(new DiffForwardTest().Get()); + _task.Wait(); + } + + public override void TestStop() => _task?.Stop(); +} + +[MovementTest(name = "差速-原地旋转2秒")] +public class DiffRotateMovementTest : MovementTest +{ + private DriveTask _task; + + public override void Test() + { + _task = new DriveTask(new DiffRotateTest().Get()); + _task.Wait(); + } + + public override void TestStop() => _task?.Stop(); +} diff --git a/DiffWheel/DiffWheelC/PilotConfig.cs b/DiffWheel/DiffWheelC/PilotConfig.cs new file mode 100644 index 0000000..9c4e911 --- /dev/null +++ b/DiffWheel/DiffWheelC/PilotConfig.cs @@ -0,0 +1,10 @@ +using ClumsyCore; +using MDCSToolBox.Clumsy.Pilot.DiffWheel; + +namespace DiffWheelC; + +public class PilotConfig : DiffWheelPilotConfig +{ + [FieldMember(desc = "差速车底盘轮距(mm)")] public float ChassisWidth = 600f; + [FieldMember(desc = "调度IP")] public string SimpleIp = "127.0.0.1"; +} diff --git a/DiffWheel/DiffWheelC/PilotDefinition.cs b/DiffWheel/DiffWheelC/PilotDefinition.cs new file mode 100644 index 0000000..e6df9e8 --- /dev/null +++ b/DiffWheel/DiffWheelC/PilotDefinition.cs @@ -0,0 +1,7 @@ +using MDCSToolBox.Clumsy.Pilot.DiffWheel; + +namespace DiffWheelC; + +public class PilotDefinition : DiffWheelPilotDefinition +{ +} diff --git a/DiffWheelM/CartDefinition.cs b/DiffWheel/DiffWheelM/CartDefinition.cs similarity index 92% rename from DiffWheelM/CartDefinition.cs rename to DiffWheel/DiffWheelM/CartDefinition.cs index 72b93c7..eba340e 100644 --- a/DiffWheelM/CartDefinition.cs +++ b/DiffWheel/DiffWheelM/CartDefinition.cs @@ -7,19 +7,19 @@ namespace DiffWheelM; public class CartDefinition : CartActivator.CartDefinition { [AsUpperIO(desc = "左轮下发速度", timeOutReset = true)] - [Sandbox] + [Playground] public float SpeedLeft; [AsUpperIO(desc = "右轮下发速度", timeOutReset = true)] - [Sandbox] + [Playground] public float SpeedRight; [AsLowerIO(desc = "左轮实际速度")] - [Sandbox] + [Playground] public float ActualSpeedLeft; [AsLowerIO(desc = "右轮实际速度")] - [Sandbox] + [Playground] public float ActualSpeedRight; [AsInitParam(desc = "手动最大轮速(m/s)")] diff --git a/MultiWheelM/MultiWheelM.csproj b/DiffWheel/DiffWheelM/DiffWheelM.csproj similarity index 77% rename from MultiWheelM/MultiWheelM.csproj rename to DiffWheel/DiffWheelM/DiffWheelM.csproj index a5a9b8b..84d679c 100644 --- a/MultiWheelM/MultiWheelM.csproj +++ b/DiffWheel/DiffWheelM/DiffWheelM.csproj @@ -6,7 +6,7 @@ enable AnyCPU false - ..\build\Medulla\plugins\ + ..\..\build\Medulla\plugins\ @@ -23,12 +23,9 @@ False - D:\MDCS\Dependencies\deps\RefFundamentalLib.dll + D:\MDCS\Release\deps\RefFundamentalLib.dll False - - D:\MDCS\Dependencies\Commons\CommonUsage.dll - diff --git a/DiffWheelM/MotorRoutine.cs b/DiffWheel/DiffWheelM/MotorRoutine.cs similarity index 63% rename from DiffWheelM/MotorRoutine.cs rename to DiffWheel/DiffWheelM/MotorRoutine.cs index 308a4bd..0e32f3d 100644 --- a/DiffWheelM/MotorRoutine.cs +++ b/DiffWheel/DiffWheelM/MotorRoutine.cs @@ -6,6 +6,7 @@ public class MotorRoutine : LadderLogic { public override void Operation(int iteration) { - + cart.ActualSpeedLeft = cart.SpeedLeft; + cart.ActualSpeedRight = cart.SpeedRight; } } diff --git a/DiffWheelM/Remote.cs b/DiffWheel/DiffWheelM/Remote.cs similarity index 100% rename from DiffWheelM/Remote.cs rename to DiffWheel/DiffWheelM/Remote.cs diff --git a/Directory.Build.props b/Directory.Build.props new file mode 100644 index 0000000..fb32865 --- /dev/null +++ b/Directory.Build.props @@ -0,0 +1,10 @@ + + + + + + $(MSBuildThisFileDirectory) + + diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs new file mode 100644 index 0000000..e4302b7 --- /dev/null +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -0,0 +1,30 @@ +using System; +using MDCSToolBox.Clumsy.AgvInterfaces; +using MDCSToolBox.Clumsy.MotionControllers; + +namespace MultiWheelC; + +public class AGV : MultiWheelInterface +{ + public override AbstractGeometricController GetController() + { + return new ChassisController().Get(); + } + + public override MultiWheelMagTracker GetMagController() + { + throw new NotImplementedException("Tutorial MultiWheelC does not implement magnetic tracking."); + } + + public override NaiveMagnetController GetNaiveMagnetController() + { + throw new NotImplementedException("Tutorial MultiWheelC does not implement naive magnetic tracking."); + } + + public void FleetGo(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId, int trackId, + float speed = -1, bool reverse = false, float carDirectionBias = 0) + { + BasicGo(srcX, srcY, srcId, dstX, dstY, dstId, trackId, speed, reverse, carDirectionBias, + multiVehicleSync: true); + } +} diff --git a/MultiWheel/MultiWheelC/ChassisController.cs b/MultiWheel/MultiWheelC/ChassisController.cs new file mode 100644 index 0000000..101f2b0 --- /dev/null +++ b/MultiWheel/MultiWheelC/ChassisController.cs @@ -0,0 +1,70 @@ +using System; +using System.Numerics; +using ClumsyCore; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using MDCSToolBox.Clumsy.MotionControllers; +using MDCSToolBox.Clumsy.Movements; +using MDCSToolBox.Clumsy.Pilot; + +namespace MultiWheelC; + +public class ChassisController : MovementDefinition +{ + public float BaseSpeed = Configuration.conf.basicSpeed; + + public override MultiWheelGeometricController Get() + { + return new MultiWheelGeometricController + { + Chassis = BasicPilotBase.Chassis, + BaseSpeed = BaseSpeed, + SlowDistance = PilotDefinition.Conf.SlowDistance, + SlowingPow = PilotDefinition.Conf.SlowingPow, + FinishDistance = PilotDefinition.Conf.FinishDistance, + FinishSpeed = PilotDefinition.Conf.FinishSpeed, + FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy, + FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac, + FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed, + NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle, + DebugMode = PilotDefinition.Conf.MotionDebugPrint, + DebugCurvature = PilotDefinition.Conf.DebugCurvature, + PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead, + SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead, + SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff, + SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff, + SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin, + SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin, + MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit, + MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit, + GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold, + DthLinearFac = PilotDefinition.Conf.DthLinearFac, + DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold, + BiasFac = PilotDefinition.Conf.BiasFac, + BiasThreshold = PilotDefinition.Conf.BiasThreshold, + + MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) => + { + PilotDefinition.Self.MultiVehicleAutoEnabled = true; + lock (PilotDefinition.Self.MultiVehicleFleet) + { + if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum) + return; + } + + PilotDefinition.Self.MultiVehicleAutoVx = speed; + PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh; + PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh; + }, + + MultiVehicleGetFleetPos = () => new Location + { + x = PilotDefinition.Self.CenterX, + y = PilotDefinition.Self.CenterY, + th = PilotDefinition.Self.CenterTh, + l_step = 1, + tick = DateTime.Now.Ticks + } + }; + } +} diff --git a/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs b/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs new file mode 100644 index 0000000..effb259 --- /dev/null +++ b/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs @@ -0,0 +1,123 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Linq; +using System.Numerics; +using System.Threading; +using ClumsyCore; +using ClumsyCore.DTools; +using ClumsyCore.Pilot; +using ClumsyCore.Utilities; +using ClumsyDance.ClumsyDance.Detectors; +using ClumsyDance.ClumsyWalk.Detectors; +using FundamentalLib; +using LineSegment = ClumsyCore.Utilities.LineSegment; + +namespace MultiWheelC; + +/// +/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。 +/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。 +/// +[MovementTest(name = "多舵轮-2腿检测")] +public class TwoLegDetect : MovementTest +{ + private Painter _painter; + private bool _running = true; + + public override void TestStop() => _running = false; + + public override void Test() + { + _running = true; + _painter = UI.GetPainter("MultiWheelTwoLegDetect", false); + _painter.Clear(); + + var lidar = PilotDefinition.Conf.TwoLegLidarName; + var lastDetectX = PilotDefinition.Conf.TwoLegGuessX; + var lastDetectY = 0f; + + while (_running) + { + var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY)); + if (ld == null) + { + Thread.Sleep(100); + continue; + } + + var center = (ld.Src + ld.Dst) / 2; + var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center); + + _painter.Clear(); + _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2); + _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2); + + Hedingben.ToastText( + $"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " + + $"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}", + "MultiWheelTwoLegDetect-test"); + + // 用本帧中心作为下一帧猜测，实现闭环跟踪 + lastDetectX = center.X; + lastDetectY = center.Y; + Thread.Sleep(100); + } + + _painter.Clear(); + } + + /// 在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。 + public static LineSegment Detect(string lidarName, float guessX, List filters) + { + var conf = PilotDefinition.Conf; +#pragma warning disable CS0612, CS0618 + var detector = new Lidar2dDetect2LegTray + { + BlobDist = conf.TwoLegBlobDist, + BlobPtCount = conf.TwoLegBlobPtCount, + BlobSize = conf.TwoLegBlobSize, + CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f), + LegWidth = conf.TwoLegWidth, + LegWidthErr = conf.TwoLegWidthErr, + Padding = conf.TwoLegPadding, + PillarFindingScope = conf.TwoLegPillarFindingScope, + SgnDir = conf.TwoLegSgnDir, + }; +#pragma warning restore CS0612, CS0618 + + return detector.DetectWithGuess( + lidarName, + new LineSegment(new Vector2(guessX, 0), Vector2.Zero), + guessCoordinateSystem: CoordinateSystem.Car2D, + outCoordinateSystem: CoordinateSystem.Car2D, + filters); + } + + /// 在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。 + public static List SetFilters(float guessCenterX, float guessCenterY) + { + var conf = PilotDefinition.Conf; + var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false); + painter.Clear(); + + var box = new[] + { + new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2), + new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2), + new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2), + new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2), + }; + + for (var i = 0; i < box.Length; ++i) + painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]); + + // 点云滤波在车体坐标系下进行 + return new List + { + new(CoordinateSystem.Car2D, + p => LessMath.IsPointInPolygon4( + box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))), + }; + } +} diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs new file mode 100644 index 0000000..1fc58c6 --- /dev/null +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -0,0 +1,241 @@ +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; + +public class MultiForwardTest : MovementDefinition +{ + public float Speed = 0.2f; + public float DurationSeconds = 2f; + + public override IEnumerable Get() + { + var chassis = (MultiWheelChassis)BasicPilotBase.Chassis; + chassis.SetOriginBias(0, 0, 0); + var end = DateTime.Now.AddSeconds(DurationSeconds); + while (DateTime.Now < end) + { + chassis.SendMotion(Speed, 0, 0); + yield return true; + } + + chassis.SendMotion(0, 0, 0); + } +} + +// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。 +public class MultiRotateToWorldAngle : MovementDefinition +{ + /// 目标朝向（世界坐标系，单位 deg）。 + public float TargetWorldDeg; + + /// 旋转角速度（deg/s，逆时针为正）。 + public float RotSpeed = 30f; + + /// 到位角度精度（deg）。 + public float ArriveDeg = 1f; + + /// 起转前舵轮对齐精度（deg）。 + public float WheelAlignDeg = 2f; + + public override IEnumerable Get() + { + var chassis = (MultiWheelChassis)BasicPilotBase.Chassis; + chassis.SetOriginBias(0, 0, 0); + + // 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。 + while (true) + { + chassis.SendRotateMotion(0); + if (WheelsAligned(chassis, WheelAlignDeg)) break; + yield return true; + } + + // 阶段二：旋转到目标世界朝向，到位即停。 + var target = CommonMath.RoundTh(TargetWorldDeg); + while (true) + { + var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th); + var diff = CommonMath.ThDiff(target, cur); // 逆时针为正 + if (Math.Abs(diff) <= ArriveDeg) break; + chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed); + yield return true; + } + + chassis.PredefinedDriveStop(); + } + + private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg) + { +#pragma warning disable CS0612, CS0618 + var wheels = chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + foreach (var sw in wheels) + if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg) + return false; + return true; + } +} + +[MovementTest(name = "多舵轮-前进2秒")] +public class MultiForwardMovementTest : MovementTest +{ + private DriveTask _task; + + public override void Test() + { + _task = new DriveTask(new MultiForwardTest().Get()); + _task.Wait(); + } + + public override void TestStop() => _task?.Stop(); +} + +[MovementTest(name = "多舵轮-原地旋转到目标角度")] +public class MultiRotateMovementTest : MovementTest +{ + private DriveTask _task; + + public override void Test() + { + _task = new DriveTask(new MultiRotateToWorldAngle + { + TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg, + RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed, + ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg, + WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg + }.Get()); + _task.Wait(); + } + + public override void TestStop() + { + _task?.Stop(); + ((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop(); + } +} + +// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）===== +// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。 + +[MovementTest(name = "多舵轮-WebAPI前移")] +public class WebApiForwardMoveTest : MovementTest +{ + public override void Test() + { + var url = PilotDefinition.Conf.PlaygroundWebApiUrl; + var name = PilotDefinition.Conf.PlaygroundRobotName; + var d = PilotDefinition.Conf.WebApiTranslateMm; + + var pose = PlaygroundWebApi.GetPose(url, name); + // 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw + var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0)); + PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg); + Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward"); + } + + public override void TestStop() + { + } +} + +[MovementTest(name = "多舵轮-WebAPI左移")] +public class WebApiLeftMoveTest : MovementTest +{ + public override void Test() + { + var url = PilotDefinition.Conf.PlaygroundWebApiUrl; + var name = PilotDefinition.Conf.PlaygroundRobotName; + var d = PilotDefinition.Conf.WebApiTranslateMm; + + var pose = PlaygroundWebApi.GetPose(url, name); + // 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧） + var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d)); + PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg); + Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft"); + } + + public override void TestStop() + { + } +} + +[MovementTest(name = "多舵轮-WebAPI旋转")] +public class WebApiRotateTest : MovementTest +{ + public override void Test() + { + var url = PilotDefinition.Conf.PlaygroundWebApiUrl; + var name = PilotDefinition.Conf.PlaygroundRobotName; + var deg = PilotDefinition.Conf.WebApiRotateDeg; + + var pose = PlaygroundWebApi.GetPose(url, name); + var ny = pose.YawDeg + deg; // 逆时针为正 + PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny); + Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate"); + } + + public override void TestStop() + { + } +} + +[MovementTest(name = "多舵轮-WebAPI恢复运动")] +public class WebApiMotionResumeTest : MovementTest +{ + public override void Test() + { + var url = PilotDefinition.Conf.PlaygroundWebApiUrl; + PlaygroundWebApi.ResumeMotion(url); + Hedingben.ToastText("已恢复车辆运动", "WebApiMotion"); + } + + public override void TestStop() + { + } +} + +[MovementTest(name = "多舵轮-WebAPI暂停运动")] +public class WebApiMotionPauseTest : MovementTest +{ + public override void Test() + { + var url = PilotDefinition.Conf.PlaygroundWebApiUrl; + PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零 + Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion"); + } + + public override void TestStop() + { + } +} + +[MovementTest(name = "多舵轮-车队直线1m")] +public class FleetStraightMovementTest : MovementTest +{ + public override void Test() + { + if (PilotDefinition.Conf.MultiVehicleMasterEndpoint != "/") + { + Hedingben.ToastText("仅主车可发起车队轨迹", "FleetGo"); + return; + } + + var pos = DetourInterface.getCartLocation(); + var agv = new AGV(); + agv.FleetGo((float)pos.x, (float)pos.y, 1, (float)pos.x + 1000f, (float)pos.y, 2, 10001, 0.2f); + } + + public override void TestStop() + { + } +} diff --git a/MultiWheel/MultiWheelC/MultiWheelC.csproj b/MultiWheel/MultiWheelC/MultiWheelC.csproj new file mode 100644 index 0000000..a279414 --- /dev/null +++ b/MultiWheel/MultiWheelC/MultiWheelC.csproj @@ -0,0 +1,45 @@ + + + + netstandard2.0 + 10 + true + false + ..\..\build\Clumsy\ + + + + + + + + + + D:\MDCS\Release\Clumsy\RefClumsyCore.dll + false + + + D:\MDCS\Release\Clumsy\RefClumsyDance.dll + false + + + D:\MDCS\Release\CommonUsage.dll + + + D:\MDCS\Release\deps\RefFundamentalLib.dll + false + + + D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll + false + + + D:\MDCS\Release\MDCSToolBox.dll + + + + + + + + diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs new file mode 100644 index 0000000..e7f9fdb --- /dev/null +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -0,0 +1,116 @@ +using ClumsyCore; +using MDCSToolBox.Clumsy.Pilot.MultiWheel; +using Newtonsoft.Json; + +namespace MultiWheelC; + +public class PilotConfig : MultiWheelPilotConfig +{ + // ===== 多车联动（已从 MDCSToolBox 内联回 Tutorial）===== + [FieldMember(desc = "[sync] 转向角加速度(deg/s^2)")] public float SyncThAccPerSec = 30f; + [FieldMember(desc = "[sync] 编队车间距(mm)")] public float TestCarSyncDistance = 2400f; + [FieldMember(desc = "[sync] 编队布局偏角(deg)")] public float TestCarSyncTh = 0f; + // 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg)，此处系数保持 1（直通），不要再次缩放。 + [FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f; + [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f; + [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f; + [FieldMember(desc = "[sync] 是否启用Detour定位")] public bool PosAvailable = true; + + [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2; + [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50; + [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/"; + [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1"; + + [JsonProperty("MultiVehicleMasterIp")] + private string LegacyMasterIpSetter + { + set + { + if (string.IsNullOrEmpty(value) || value == "/") return; + if (MultiVehicleMasterEndpoint == "/") + MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008"; + } + } + + [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false; + [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f; + [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f; + [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f; + [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f; + [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f; + [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f; + + [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f; + [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f; + [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f; + [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f; + [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f; + [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f; + + [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f; + [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f; + + [FieldMember(desc = "Playground WebAPI 基地址")] + public string PlaygroundWebApiUrl = "http://localhost:18090"; + + [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")] + public string PlaygroundRobotName = "agv_multi_1"; + + [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")] + public float WebApiTranslateMm = 100f; + + [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")] + public float WebApiRotateDeg = 5f; + + [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")] + public float InPlaceRotateTargetWorldDeg = 90f; + + [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")] + public float InPlaceRotateSpeed = 30f; + + [FieldMember(desc = "原地旋转：到位角度精度(deg)")] + public float InPlaceRotateArriveDeg = 1f; + + [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")] + public float InPlaceRotateWheelAlignDeg = 2f; + + // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）===== + [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")] + public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1"; + + [FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")] + public float TwoLegGuessX = 2000f; + + [FieldMember(desc = "2腿检测：两腿间距(mm)")] + public float TwoLegWidth = 800f; + + [FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")] + public float TwoLegWidthErr = 100f; + + [FieldMember(desc = "2腿检测：聚类点间距(mm)")] + public float TwoLegBlobDist = 100f; + + [FieldMember(desc = "2腿检测：聚类尺寸(mm)")] + public float TwoLegBlobSize = 200f; + + [FieldMember(desc = "2腿检测：聚类最小点数")] + public int TwoLegBlobPtCount = 5; + + [FieldMember(desc = "2腿检测：聚类 padding")] + public int TwoLegPadding = 5; + + [FieldMember(desc = "2腿检测：腿柱搜索范围")] + public int TwoLegPillarFindingScope = 20; + + [FieldMember(desc = "2腿检测：方向符号(±1)")] + public int TwoLegSgnDir = 1; + + [FieldMember(desc = "2腿检测：中心X偏移(mm)")] + public float TwoLegCenterChangeX = 0f; + + [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")] + public float TwoLegFilterLength = 1800f; + + [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")] + public float TwoLegFilterWidth = 600f; +} diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs new file mode 100644 index 0000000..6f30af7 --- /dev/null +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -0,0 +1,773 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Linq; +using System.Net.Http; +using System.Numerics; +using System.Threading; +using System.Threading.Tasks; +using ClumsyCore; +using ClumsyCore.DTools; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using ClumsyCore.Utilities; +using CommonUsage; +using CommonUsage.Chassis; +using FundamentalLib; +using FundamentalLib.Utilities; +using MDCSToolBox.Clumsy.Pilot.MultiWheel; +using Newtonsoft.Json; +using Vector = ClumsyCore.Utilities.Vector; + +namespace MultiWheelC; + +public class PilotDefinition : MultiWheelPilotDefinition +{ + public new float CarLength = 1472f; + public new float CarWidth = 948f; + + public override void StandardInit() + { + InitMultiVehicleCoordination(); + } + + #region MultiVehicleCoordination (从 MDCSToolBox 内联回 Tutorial) + + [AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled; + [AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode; + [FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned; + + [AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx; + [AsLowerIO(desc = "多车联动：遥控器Vy")] public float MultiVehicleManualVy; + [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth; + + [AsUpperIO(desc = "多车联动：自动驾驶", timeOutReset = true)] public bool MultiVehicleAutoEnabled; + [FieldMember(desc = "多车联动：自动驾驶Vx")] public float MultiVehicleAutoVx; + [FieldMember(desc = "多车联动：自动驾驶FrontTh")] public float MultiVehicleAutoFrontTh; + [FieldMember(desc = "多车联动：自动驾驶RearTh")] public float MultiVehicleAutoRearTh; + + public Dictionary MultiVehicleFleet = new(); + public VehicleSyncNotification MultiVehicleNotification; + + [FieldMember(desc = "多车联动：车队姿态x")] public float CenterX; + [FieldMember(desc = "多车联动：车队姿态y")] public float CenterY; + [FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh; + + [AsLowerIO(desc = "车号")] public int CarNum = 1; + + private float _multiVehicleAccumulateTh; + private DateTime _multiVehicleLastThTime = DateTime.Now; + private readonly object _multiVehicleNotificationLock = new(); + private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue; + private bool _multiVehicleSyncInitialized; + + // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源 + private DateTime _mvDbgLastLog = DateTime.MinValue; + private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist; + + // 写盘诊断日志（Clumsy 侧）。文件落在 Clumsy 工作目录，复现后离线分析。 + // 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。 + private DateTime _mvDiskLastLog = DateTime.MinValue; + private static bool _fleetDiagInit; + private void FleetDiag(string msg) + { + try + { + const string path = "fleet_diag_clumsy.log"; + var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}"; + if (!_fleetDiagInit) + { + _fleetDiagInit = true; + System.IO.File.WriteAllText(path, + $"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line); + } + else + System.IO.File.AppendAllText(path, line); + } + catch + { + // 诊断日志失败不影响主流程 + } + } + + // 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动 + private readonly object _neighborDetectLock = new(); + private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new(); + + // 互识别：上一帧检测中心，用作下一帧猜测（闭环跟踪），与「多舵轮-2腿检测」MovementTest 一致 + private Vector2 _neighborGuess; + private bool _neighborGuessValid; + private DateTime _neighborGuessTime = DateTime.MinValue; + + /// + /// 互识别钩子：用单线雷达识别邻车的两腿/两轮，换算成本车体坐标系下相对“理应对齐参考点”的偏差。 + /// 全对齐时返回 (true, 0, 0, 0)；未检测到时返回 (false, ...)。 + /// detectDistance = 编队车间距 - 检测中心偏移（即邻车检测中心到本车原点的标称距离，恒为正值，方向由 TwoLegGuessX 符号决定）。 + /// + private (bool Valid, float Dx, float Dy, float Dth, float NeighborDist) DetectNeighborBias(float detectDistance) + { + // 检测猜测点与「多舵轮-2腿检测」MovementTest 完全一致：首帧 / 跟丢回落都用 Conf.TwoLegGuessX + // （方向与距离都由它一处决定），跟到后用上一帧中心闭环跟踪。 + // detectDistance 只用于计算编队对齐偏差/间距，不参与“去哪里找邻车”，避免猜测点偏离导致 ROI 框漏掉真实邻车。 + var nominal = new Vector2(Conf.TwoLegGuessX, 0f); + + var now = DateTime.Now; + // 闭环跟踪：1s 内检到过就用上一帧中心作猜测，使绿色 ROI 框收敛到真实邻车（与 MovementTest 行为一致）； + // 跟丢超时则回落到编队标称位置，避免框漂走。 + var guess = (_neighborGuessValid && (now - _neighborGuessTime).TotalSeconds < 1) ? _neighborGuess : nominal; + var seg = TwoLegDetect.Detect(Conf.TwoLegLidarName, guess.X, TwoLegDetect.SetFilters(guess.X, guess.Y)); + + Vector2 avgSrc, avgDst; + lock (_neighborDetectLock) + { + if (seg != null) + { + _neighborDetects.Add((now, seg.Src, seg.Dst)); + _neighborGuess = (seg.Src + seg.Dst) / 2f; + _neighborGuessValid = true; + _neighborGuessTime = now; + } + _neighborDetects.RemoveAll(r => (now - r.Time).TotalSeconds > 1); + + var cnt = _neighborDetects.Count; + if (cnt == 0) + { + _neighborGuessValid = false; + Hedingben.ToastText($"邻车未检到 (guess x:{guess.X:F0} y:{guess.Y:F0})", $"MultiVehicle{CarNum}-detect"); + return (false, 0f, 0f, 0f, 0f); + } + if (cnt > 10) _neighborDetects.RemoveRange(0, cnt - 10); + + avgSrc = new Vector2(_neighborDetects.Average(r => r.Src.X), _neighborDetects.Average(r => r.Src.Y)); + avgDst = new Vector2(_neighborDetects.Average(r => r.Dst.X), _neighborDetects.Average(r => r.Dst.Y)); + } + + // 两腿连线中点为邻车检测中心；连线的垂线方向即邻车朝向 + var dirVec = avgDst - avgSrc; + var center = (avgSrc + avgDst) / 2f; + var dir = (float)LessMath.RoundTh(Math.Atan2(dirVec.Y, dirVec.X) / Math.PI * 180 + 90); + + // 从检测中心沿邻车朝向外推 detectDistance，得到“本车理应所在的参考点”（车体系） + var target = LessMath.Transform2D(center, dir, detectDistance, 0); + + var dx = target.X; + var dy = target.Y; + var dth = (float)LessMath.ThDiff(dir, 0); + var neighborDist = center.Length(); + + _mvLastDetCenterX = center.X; + _mvLastDetCenterY = center.Y; + _mvLastDetDir = dir; + _mvLastDetDist = neighborDist; + + var painter = UI.GetPainter("MultiVehicleDetectBias", false); + painter.Clear(); + painter.DrawLine(Color.DeepPink, center, target, width: 2); + painter.DrawText(Color.Yellow, $"dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}", center.X, center.Y); + + Hedingben.ToastText( + $"[联动检测] lidar:{Conf.TwoLegLidarName} guess x:{guess.X:F0} y:{guess.Y:F0} | " + + $"中心 x:{center.X:F0} y:{center.Y:F0} dir:{dir:F1} dist:{neighborDist:F0} | 偏差 dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}", + $"MultiVehicle{CarNum}-detect"); + + return (true, dx, dy, dth, neighborDist); + } + + private void InitMultiVehicleCoordination() + { + if (_multiVehicleSyncInitialized) return; + _multiVehicleSyncInitialized = true; + + var hc = new HttpClient { Timeout = TimeSpan.FromSeconds(2) }; + + PicoHttpServer.AddGetHandler("/multi-vehicle-register", new { CarNum = 0, Info = "" }, query => + { + try + { + var infoJson = Uri.UnescapeDataString(query.Info ?? ""); + var info = JsonConvert.DeserializeObject(infoJson) + ?? throw new Exception("VehicleSyncInfo is null"); + lock (MultiVehicleFleet) + MultiVehicleFleet[query.CarNum] = info; + return JsonConvert.SerializeObject(new { code = 200, message = "ok" }); + } + catch (Exception e) + { + DLog.Log($"/multi-vehicle-register error: {e.FormatEx()}", "MultiVehicle"); + return JsonConvert.SerializeObject(new { code = 500, message = e.Message }); + } + }); + + PicoHttpServer.AddGetHandler("/multi-vehicle-notify", new { Notification = "" }, query => + { + try + { + var notifyJson = Uri.UnescapeDataString(query.Notification ?? ""); + var notification = JsonConvert.DeserializeObject(notifyJson) + ?? throw new Exception("notification is null"); + MultiVehicleAligned = notification.Aligned; + lock (MultiVehicleFleet) + MultiVehicleFleet = notification.Fleet ?? new Dictionary(); + lock (_multiVehicleNotificationLock) + { + MultiVehicleNotification = notification; + _multiVehicleLastNotifyTime = DateTime.Now; + } + return JsonConvert.SerializeObject(new { code = 200, message = "ok" }); + } + catch (Exception e) + { + DLog.Log($"/multi-vehicle-notify error: {e.FormatEx()}", "MultiVehicle"); + return JsonConvert.SerializeObject(new { code = 500, message = e.Message }); + } + }); + + new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start(); + } + + private void MultiVehicleLoop(HttpClient hc) + { + var carLength = CarLength; + var carWidth = CarWidth; + var contour = new List + { + new(carLength / 2f, carWidth / 2f), + new(-carLength / 2f, carWidth / 2f), + new(-carLength / 2f, -carWidth / 2f), + new(carLength / 2f, -carWidth / 2f), + }; + var chassis = (MultiWheelChassis)Chassis; + var sendMotionPainter = UI.GetPainter("MultiWheelChassis-SendMotionVis", false); + chassis.SendMotionVisualizer = new Visualizer + { + LineAction = (color, src, dst, startArrow, endArrow, width) => + sendMotionPainter.DrawLine(color, src, dst, startArrow, endArrow, width), + TextAction = (color, str, pos) => sendMotionPainter.DrawText(color, str, pos), + Clear = () => sendMotionPainter.Clear() + }; + + while (true) + { + try + { + TickMultiVehicle(hc, chassis, contour, sendMotionPainter); + } + catch (Exception e) + { + DLog.Log($"MultiVehicle loop error: {e.FormatEx()}", "MultiVehicle"); + } + + Thread.Sleep(Conf.MultiVehicleSyncInterval); + } + } + + private void TickMultiVehicle(HttpClient hc, MultiWheelChassis chassis, List contour, Painter sendMotionPainter) + { + var isMaster = IsMultiVehicleMaster(); + var autoEnabled = false; + var manualEnabled = MultiVehicleManualEnabled; + + if (isMaster) + autoEnabled = MultiVehicleAutoEnabled; + else + { + // 从车：自动/手动联动均可由主车广播解锁（无需各自再拨开关）。 + // 仅在 notification 新鲜时认账，主车停发后超时即自动停车，避免用旧指令跑飞。 + lock (_multiVehicleNotificationLock) + { + var fresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds < + Math.Max(300, Conf.MultiVehicleSyncInterval * 5); + if (fresh && MultiVehicleNotification != null) + { + autoEnabled = MultiVehicleNotification.AutoEnabled; + manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled; + } + } + } + + // 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定， + // 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。 + if ((DateTime.Now - _mvDiskLastLog).TotalMilliseconds >= 300) + { + _mvDiskLastLog = DateTime.Now; + int fleetCnt; + lock (MultiVehicleFleet) fleetCnt = MultiVehicleFleet.Count; + var notifFresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds < + Math.Max(300, Conf.MultiVehicleSyncInterval * 5); + FleetDiag( + $"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " + + $"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " + + $"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " + + $"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " + + $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} pos={Conf.PosAvailable}"); + } + + if (!manualEnabled && !autoEnabled) + { + UI.GetPainter("MultiVehicleFleet-vis", false).Clear(); + sendMotionPainter.Clear(); + lock (MultiVehicleFleet) + MultiVehicleFleet.Clear(); + MultiVehicleAutoEnabled = false; + _multiVehicleAccumulateTh = 0f; + _multiVehicleLastThTime = DateTime.Now; + + if (!isMaster) + FireAndForgetRegister(hc, BuildSelfInfo(false, false, 0, 0, 0, 0, 0, 0, false)); + + return; + } + + if (isMaster && autoEnabled && !MultiVehicleManualEnabled && !FleetHasPosAvailable()) + { + MultiVehicleAutoEnabled = false; + Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate"); + return; + } + + var posAvailable = Conf.PosAvailable; + float selfX = 0, selfY = 0, selfTh = 0; + if (posAvailable) + { + var carPos = DetourInterface.getCartLocation(); + selfX = (float)carPos.x; + selfY = (float)carPos.y; + selfTh = (float)carPos.th; + } + + var syncTh = Conf.TestCarSyncTh; + var syncDistance = Conf.TestCarSyncDistance; + var deltaDetectCenter = Conf.DeltaDetectCenter; + float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0; + + CenterX = CenterY = CenterTh = 0; + + if (isMaster) + { + if (MultiVehicleManualEnabled) + { + syncTh = 0; + fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac; + var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac; + var now = DateTime.Now; + var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds)); + _multiVehicleLastThTime = now; + var sign = Math.Sign(targetTh - _multiVehicleAccumulateTh); + _multiVehicleAccumulateTh += sign * Math.Min(Math.Abs(targetTh - _multiVehicleAccumulateTh), + Conf.SyncThAccPerSec * dt); + fleetFrontTh = _multiVehicleAccumulateTh; + fleetRearTh = -fleetFrontTh; + } + else + { + fleetVx = MultiVehicleAutoVx; + fleetFrontTh = MultiVehicleAutoFrontTh; + fleetRearTh = MultiVehicleAutoRearTh; + } + } + else if (MultiVehicleNotification != null) + { + VehicleSyncNotification notification; + lock (_multiVehicleNotificationLock) + notification = MultiVehicleNotification; + + syncTh = notification.SyncTh; + syncDistance = notification.SyncDistance; + deltaDetectCenter = notification.DeltaDetectCenter; + CenterX = notification.CenterX; + CenterY = notification.CenterY; + CenterTh = notification.CenterTh; + posAvailable = notification.PosAvailable; + MultiVehicleAutoEnabled = notification.AutoEnabled; + fleetVx = notification.FleetVx; + fleetFrontTh = notification.FleetFrontTh; + fleetRearTh = notification.FleetRearTh; + } + + var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance); + + if (isMaster) + { + lock (MultiVehicleFleet) + MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh, + layoutX, layoutY, layoutTh, true); + + if (TryInferFleetCenter(out var cx, out var cy, out var cth)) + { + CenterX = cx; + CenterY = cy; + CenterTh = cth; + } + } + + var selfAligned = !Conf.MultiVehicleUseDetect; + var detectValid = false; + float detectDx = 0, detectDy = 0, detectDth = 0; + var currentSpacing = float.NaN; + if (Conf.MultiVehicleUseDetect) + { + var (valid, dx, dy, dth, neighborDist) = DetectNeighborBias(syncDistance - deltaDetectCenter); + detectValid = valid; + if (valid) + { + detectDx = dx; + detectDy = dy; + detectDth = dth; + // 检测中心到本车原点距离 + 检测中心偏移 = 两车参考点当前间距 + currentSpacing = neighborDist + deltaDetectCenter; + selfAligned = Math.Abs(dx) < Conf.SingleCarSyncPrecisionXy && + Math.Abs(dy) < Conf.SingleCarSyncPrecisionXy && + Math.Abs(dth) < Conf.SingleCarSyncPrecisionTh; + } + else selfAligned = false; + } + + // 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（两台车都需有定位） + if (float.IsNaN(currentSpacing) && posAvailable) + currentSpacing = TryGetSlamSpacing(selfX, selfY); + + if (float.IsNaN(currentSpacing)) + Hedingben.ToastText($"间距 当前:N/A 目标:{syncDistance:F0}mm", $"MultiVehicle{CarNum}-spacing"); + else + Hedingben.ToastText( + $"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm", + $"MultiVehicle{CarNum}-spacing"); + + lock (MultiVehicleFleet) + { + MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum && + MultiVehicleFleet.Values.All(v => v.Aligned); + } + + // 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。 + // ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。 + var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid; + bool othersDetectOk; + lock (MultiVehicleFleet) + othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk); + var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk); + if (!canMove) + { + fleetVx = 0; + fleetFrontTh = 0; + fleetRearTh = 0; + Hedingben.ToastText( + $"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）", + $"MultiVehicle{CarNum}-stop"); + } + else + { + Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop"); + } + + VisualizeFleet(contour, layoutX, layoutY, layoutTh); + + var fleetReady = false; + var fleetCount = 0; + lock (MultiVehicleFleet) + { + fleetCount = MultiVehicleFleet.Count; + fleetReady = fleetCount == Conf.MultiVehicleFleetNum; + } + + // 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。 + float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0; + float xPosCompensate = 0, yPosCompensate = 0, thPosCompensate = 0; + float posBiasX = 0, posBiasY = 0, posBiasTh = 0; + + if (fleetReady) + { + chassis.SetOriginBias(layoutX, layoutY, layoutTh); + chassis.ControlPointRadius = syncDistance / 2f; + + if (canMove && Conf.MultiVehicleUseDetect) + { + xDetectCompensate = Math.Abs(detectDx) > Conf.SingleCarSyncPrecisionXy + ? detectDx * Conf.MultiVehicleDetectBiasXFac : 0; + yDetectCompensate = Math.Abs(detectDy) > Conf.SingleCarSyncPrecisionXy + ? detectDy * Conf.MultiVehicleDetectBiasYFac : 0; + thDetectCompensate = Math.Abs(detectDth) > Conf.SingleCarSyncPrecisionTh + ? detectDth * Conf.MultiVehicleDetectBiasThFac : 0; + + xDetectCompensate = ClampBias(xDetectCompensate, Conf.MultiVehicleDetectBiasXThreshold); + yDetectCompensate = ClampBias(yDetectCompensate, Conf.MultiVehicleDetectBiasYThreshold); + thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold); + } + + if (canMove && posAvailable && TryInferFleetCenter(out _, out _, out _)) + { + var supposedPos = LessMath.Transform2D( + Tuple.Create(CenterX, CenterY, CenterTh), + Tuple.Create(layoutX, layoutY, layoutTh)); + var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos); + posBiasX = (float)posBias.Item1; + posBiasY = (float)posBias.Item2; + posBiasTh = (float)posBias.Item3; + + xPosCompensate = Math.Abs(posBias.Item1) > Conf.SingleCarSyncPrecisionXy + ? ClampBias(posBias.Item1 * Conf.MultiVehiclePosBiasXFac, Conf.MultiVehiclePosBiasXThreshold) + : 0; + yPosCompensate = Math.Abs(posBias.Item2) > Conf.SingleCarSyncPrecisionXy + ? ClampBias(posBias.Item2 * Conf.MultiVehiclePosBiasYFac, Conf.MultiVehiclePosBiasYThreshold) + : 0; + thPosCompensate = Math.Abs(posBias.Item3) > Conf.SingleCarSyncPrecisionTh + ? ClampBias(posBias.Item3 * Conf.MultiVehiclePosBiasThFac, Conf.MultiVehiclePosBiasThThreshold) + : 0; + } + + sendMotionPainter.Clear(); + chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510, + localCompensateX: xDetectCompensate + xPosCompensate, + localCompensateY: yDetectCompensate + yPosCompensate, + localCompensateTh: thDetectCompensate + thPosCompensate); + } + + // === 诊断日志（节流 ~300ms）=== + // 用于定位“剧烈运动”来源：BASE=遥控基础速度；DETECT=2腿检测补偿；POS=SLAM编队补偿；SEND=最终下发。 + // 若 BASE≈0 但 SEND 持续非零，说明是补偿在驱动；再看 DETECT/POS 哪一路的 comp 持续非零即定位到来源。 + // 同时落 DLog(tag MultiVehicleDbg) 与屏幕 ToastText(tag MultiVehicle{CarNum}-dbg)，后者无需开启磁盘转储即可直接观察。 + if ((DateTime.Now - _mvDbgLastLog).TotalMilliseconds >= 300) + { + _mvDbgLastLog = DateTime.Now; + var spacingStr = float.IsNaN(currentSpacing) ? "NaN" : currentSpacing.ToString("F0"); + var cx = xDetectCompensate + xPosCompensate; + var cy = yDetectCompensate + yPosCompensate; + var cth = thDetectCompensate + thPosCompensate; + + var dbg = + $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} pos:{posAvailable} " + + $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " + + $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " + + $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " + + $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} " + + $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " + + $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " + + $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " + + $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " + + $"| SEND vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2}"; + + DLog.Log(dbg, "MultiVehicleDbg"); + FleetDiag(dbg); + + // 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储） + Hedingben.ToastText( + $"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " + + $"| ready{fleetReady} canMove{canMove}", + $"MultiVehicle{CarNum}-dbg"); + Hedingben.ToastText( + $"DET v{detectValid} dx{detectDx:F0} dy{detectDy:F0} dth{detectDth:F1} cmp({xDetectCompensate:F0},{yDetectCompensate:F0},{thDetectCompensate:F1}) " + + $"| POS bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) cmp({xPosCompensate:F0},{yPosCompensate:F0},{thPosCompensate:F1})", + $"MultiVehicle{CarNum}-dbg2"); + } + + if (isMaster) + { + lock (MultiVehicleFleet) + { + MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh, + layoutX, layoutY, layoutTh, selfAligned, ownDetectOk); + } + + if (fleetReady) + { + VehicleSyncNotification notification; + lock (MultiVehicleFleet) + { + notification = new VehicleSyncNotification + { + PosAvailable = posAvailable, + CenterX = CenterX, + CenterY = CenterY, + CenterTh = CenterTh, + Aligned = MultiVehicleAligned, + Fleet = new Dictionary(MultiVehicleFleet), + FleetVx = fleetVx, + FleetFrontTh = fleetFrontTh, + FleetRearTh = fleetRearTh, + AutoEnabled = MultiVehicleAutoEnabled, + ManualEnabled = MultiVehicleManualEnabled, + SyncTh = syncTh, + SyncDistance = syncDistance, + DeltaDetectCenter = deltaDetectCenter + }; + } + + foreach (var kv in notification.Fleet) + { + var ip = kv.Value.Ip; + var port = kv.Value.Port > 0 ? kv.Value.Port : WebAPI.port; + if (string.IsNullOrWhiteSpace(ip) || kv.Key == CarNum) + continue; + FireAndForgetNotify(hc, ip, port, notification); + } + } + } + else + { + FireAndForgetRegister(hc, BuildSelfInfo(false, posAvailable, selfX, selfY, selfTh, + layoutX, layoutY, layoutTh, selfAligned, ownDetectOk)); + } + } + + private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/"; + + private bool FleetHasPosAvailable() + { + lock (MultiVehicleFleet) + return MultiVehicleFleet.Values.Any(v => v.PosAvailable); + } + + // 基于 SLAM 世界坐标的最近邻车间距（mm）；无可用定位邻车时返回 NaN。 + private float TryGetSlamSpacing(float selfX, float selfY) + { + List others; + lock (MultiVehicleFleet) + others = MultiVehicleFleet + .Where(kv => kv.Key != CarNum && kv.Value.PosAvailable) + .Select(kv => kv.Value).ToList(); + + if (others.Count == 0) return float.NaN; + return others.Min(o => (float)Math.Sqrt((o.X - selfX) * (o.X - selfX) + (o.Y - selfY) * (o.Y - selfY))); + } + + private bool TryInferFleetCenter(out float centerX, out float centerY, out float centerTh) + { + centerX = centerY = centerTh = 0; + List positioned; + lock (MultiVehicleFleet) + positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList(); + + if (positioned.Count == 0) return false; + + var centers = positioned.Select(InferFleetCenterFromCar).ToList(); + centerX = centers.Average(c => c.Item1); + centerY = centers.Average(c => c.Item2); + var avgSin = centers.Average(c => Math.Sin(c.Item3 * Math.PI / 180.0)); + var avgCos = centers.Average(c => Math.Cos(c.Item3 * Math.PI / 180.0)); + centerTh = (float)(Math.Atan2(avgSin, avgCos) * 180.0 / Math.PI); + return true; + } + + private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car) + { + // 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。 + // 注意 layout⁻¹ 是真正的 SE(2) 逆，而非逐分量取负 (-layout)： + // 当 layoutTh≠0（如从车 180°）时，逐分量取负会把平移方向算错，导致车队中心偏移 → 位姿补偿跑飞。 + var layout = Tuple.Create((double)car.LayoutX, (double)car.LayoutY, (double)car.LayoutTh); + var layoutInv = LessMath.SolveTransform2D(layout, Tuple.Create(0.0, 0.0, 0.0)); + var center = LessMath.Transform2D( + Tuple.Create((double)car.X, (double)car.Y, (double)car.Th), + layoutInv); + return ((float)center.Item1, (float)center.Item2, (float)center.Item3); + } + + private (float, float, float) GetLayoutPose(float syncTh, float syncDistance) + { + // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定： + // 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。 + var rad = syncTh / 180f * Math.PI; + var sign = CarNum == 1 ? 1f : -1f; + var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign); + var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign); + return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180)); + } + + private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th, + float layoutX, float layoutY, float layoutTh, bool aligned, bool detectOk = true) + { + return new VehicleSyncInfo + { + Master = master, + Ip = Conf.SimpleIp, + Port = WebAPI.port, + PosAvailable = posAvailable, + X = x, + Y = y, + Th = th, + LayoutX = layoutX, + LayoutY = layoutY, + LayoutTh = layoutTh, + Aligned = aligned, + DetectOk = detectOk + }; + } + + private void VisualizeFleet(List contour, float egoLayoutX, float egoLayoutY, float egoLayoutTh) + { + // 画在本车车体系：global=false，渲染时由 ClumsyLite 叠加本车真实位姿。 + // 每个成员按“相对本车 layout 的位姿”绘制（memberInEgo = egoLayout⁻¹ ∘ memberLayout）， + // 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。 + var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false); + Dictionary fleet; + lock (MultiVehicleFleet) + fleet = new Dictionary(MultiVehicleFleet); + + var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh); + + Tuple MemberInEgo(float mx, float my, float mth) + => LessMath.SolveTransform2D(egoLayout, Tuple.Create((double)mx, (double)my, (double)mth)); + + void DrawContour(Color color, Tuple rel) + { + var transformed = contour + .Select(pp => LessMath.Transform2D((float)rel.Item1, (float)rel.Item2, (float)rel.Item3, pp)) + .ToList(); + for (var i = 0; i < 4; ++i) + fleetPainter.DrawLine(color, transformed[i], transformed[(i + 1) % 4]); + // 对角线便于辨认朝向 + fleetPainter.DrawLine(color, transformed[0], transformed[2]); + fleetPainter.DrawLine(color, transformed[1], transformed[3]); + } + + fleetPainter.Clear(); + foreach (var kvp in fleet) + { + var color = kvp.Value.Master ? Color.Red : Color.Orange; + var rel = MemberInEgo(kvp.Value.LayoutX, kvp.Value.LayoutY, kvp.Value.LayoutTh); + DrawContour(color, rel); + fleetPainter.DrawText(color, $"{kvp.Key}", new Vector((float)rel.Item1, (float)rel.Item2)); + } + } + + private void FireAndForgetRegister(HttpClient hc, VehicleSyncInfo info) + { + ParseMasterEndpoint(out var masterIp, out var masterPort); + var infoJson = JsonConvert.SerializeObject(info); + var url = + $"http://{masterIp}:{masterPort}/multi-vehicle-register?CarNum={CarNum}&Info={Uri.EscapeDataString(infoJson)}"; + _ = hc.GetStringAsync(url).ContinueWith(t => + { + if (t.IsFaulted) + DLog.Log($"register failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle"); + }, TaskScheduler.Default); + } + + private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification) + { + var notifyJson = JsonConvert.SerializeObject(notification); + var url = $"http://{ip}:{port}/multi-vehicle-notify?Notification={Uri.EscapeDataString(notifyJson)}"; + _ = hc.GetStringAsync(url).ContinueWith(t => + { + if (t.IsFaulted) + DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle"); + }, TaskScheduler.Default); + } + + private void ParseMasterEndpoint(out string ip, out int port) + { + ip = "127.0.0.1"; + port = 8008; + var endpoint = Conf.MultiVehicleMasterEndpoint ?? "/"; + if (endpoint == "/") return; + var parts = endpoint.Split(':'); + if (parts.Length >= 1 && !string.IsNullOrWhiteSpace(parts[0])) + ip = parts[0]; + if (parts.Length >= 2 && int.TryParse(parts[1], out var p)) + port = p; + } + + private static float ClampBias(float value, float threshold) + => Math.Sign(value) * Math.Min(Math.Abs(value), threshold); + + #endregion +} diff --git a/MultiWheel/MultiWheelC/PlaygroundWebApi.cs b/MultiWheel/MultiWheelC/PlaygroundWebApi.cs new file mode 100644 index 0000000..03ace8e --- /dev/null +++ b/MultiWheel/MultiWheelC/PlaygroundWebApi.cs @@ -0,0 +1,81 @@ +using System; +using System.Net.Http; +using System.Text; +using Newtonsoft.Json; +using Newtonsoft.Json.Linq; + +namespace MultiWheelC; + +/// +/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。 +/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。 +/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。 +/// +public static class PlaygroundWebApi +{ + // 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。 + private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false }) + { + Timeout = TimeSpan.FromSeconds(3) + }; + + public struct Pose + { + public float X; + public float Y; + public float YawDeg; + } + + /// 查询单台小车的世界位姿。GET /api/robots/{name}。 + public static Pose GetPose(string baseUrl, string robotName) + { + var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}"; + var json = Http.GetStringAsync(url).GetAwaiter().GetResult(); + var o = JObject.Parse(json); + return new Pose + { + X = o.Value("x"), + Y = o.Value("y"), + YawDeg = o.Value("yawDeg") + }; + } + + /// 将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。 + public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg) + { + var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move"; + var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true }); + using var content = new StringContent(body, Encoding.UTF8, "application/json"); + var resp = Http.PostAsync(url, content).GetAwaiter().GetResult(); + resp.EnsureSuccessStatusCode(); + } + + /// 查询车辆运动是否启用（暂停时为 false）。GET /api/motion。 + public static bool MotionEnabled(string baseUrl) + { + var url = $"{baseUrl.TrimEnd('/')}/api/motion"; + var json = Http.GetStringAsync(url).GetAwaiter().GetResult(); + return JObject.Parse(json).Value("motionEnabled"); + } + + /// 恢复车辆运动。POST /api/motion/resume。 + public static void ResumeMotion(string baseUrl) + { + var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume"; + var resp = Http.PostAsync(url, null).GetAwaiter().GetResult(); + resp.EnsureSuccessStatusCode(); + } + + /// + /// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。 + /// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。 + /// + public static void PauseMotion(string baseUrl, string feedback = "zero") + { + var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause"; + var body = JsonConvert.SerializeObject(new { feedback }); + using var content = new StringContent(body, Encoding.UTF8, "application/json"); + var resp = Http.PostAsync(url, content).GetAwaiter().GetResult(); + resp.EnsureSuccessStatusCode(); + } +} diff --git a/MultiWheel/MultiWheelC/VehicleSyncModels.cs b/MultiWheel/MultiWheelC/VehicleSyncModels.cs new file mode 100644 index 0000000..ab69645 --- /dev/null +++ b/MultiWheel/MultiWheelC/VehicleSyncModels.cs @@ -0,0 +1,40 @@ +using System.Collections.Generic; +using ClumsyCore; +using Newtonsoft.Json; + +namespace MultiWheelC; + +public class VehicleSyncInfo +{ + [JsonProperty("Master")] public bool Master { get; set; } + [JsonProperty("Ip")] public string Ip { get; set; } = ""; + [JsonProperty("Port")] public int Port { get; set; } = 8008; + [JsonProperty("PosAvailable")] public bool PosAvailable { get; set; } + [JsonProperty("X")] public float X { get; set; } + [JsonProperty("Y")] public float Y { get; set; } + [JsonProperty("Th")] public float Th { get; set; } + [JsonProperty("LayoutX")] public float LayoutX { get; set; } + [JsonProperty("LayoutY")] public float LayoutY { get; set; } + [JsonProperty("LayoutTh")] public float LayoutTh { get; set; } + [JsonProperty("Aligned")] public bool Aligned { get; set; } + // 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。 + [JsonProperty("DetectOk")] public bool DetectOk { get; set; } +} + +public class VehicleSyncNotification +{ + [JsonProperty("PosAvailable")] public bool PosAvailable { get; set; } + [JsonProperty("CenterX")] public float CenterX { get; set; } + [JsonProperty("CenterY")] public float CenterY { get; set; } + [JsonProperty("CenterTh")] public float CenterTh { get; set; } + [JsonProperty("Aligned")] public bool Aligned { get; set; } + [JsonProperty("Fleet")] public Dictionary Fleet { get; set; } = new(); + [JsonProperty("FleetVx")] public float FleetVx { get; set; } + [JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; } + [JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; } + [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; } + [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; } + [JsonProperty("SyncTh")] public float SyncTh { get; set; } + [JsonProperty("SyncDistance")] public float SyncDistance { get; set; } + [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; } +} diff --git a/MultiWheelM/CartDefinition.cs b/MultiWheel/MultiWheelM/CartDefinition.cs similarity index 82% rename from MultiWheelM/CartDefinition.cs rename to MultiWheel/MultiWheelM/CartDefinition.cs index b3bb9f7..57e7418 100644 --- a/MultiWheelM/CartDefinition.cs +++ b/MultiWheel/MultiWheelM/CartDefinition.cs @@ -6,8 +6,10 @@ namespace MultiWheelM; [UseManualController(manualController = typeof(Remote))] [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)] -public class CartDefinition : CartActivator.CartDefinition +public partial class CartDefinition : CartActivator.CartDefinition { + [AsLowerIO(desc = "GhostMode")] public bool GhostMode = false; + [AsUpperIO(desc = "左前轮速度", timeOutReset = true)] [Playground] public float SpeedLeftFront; @@ -75,6 +77,27 @@ public class CartDefinition : CartActivator.CartDefinition [AsLowerIO(desc = "遥控模式")] public int ManualMode = -1; + // 车号：在 Medulla 初始化参数面板可视化配置(AsInitParam)，并作为下位信号上报给 Clumsy(AsLowerIO)。 + [AsLowerIO(desc = "车号")] + [AsInitParam(desc = "车号")] + public int CarNum = 1; + + // 手动多车联动指令由遥控器在 Medulla 侧产生、上报给 Clumsy，方向为下位->上位，故统一为 AsLowerIO。 + [AsLowerIO(desc = "启用（手动）多车联动")] + public bool MultiVehicleManualEnabled; + + [AsLowerIO(desc = "（手动）多车联动模式")] + public int MultiVehicleManualMode; + + [AsLowerIO(desc = "多车联动：遥控器Vx")] + public float MultiVehicleManualVx; + + [AsLowerIO(desc = "多车联动：遥控器Vy")] + public float MultiVehicleManualVy; + + [AsLowerIO(desc = "多车联动：遥控器Vth")] + public float MultiVehicleManualVth; + [AsInitParam(desc = "手动最大速度(m/s)")] public float MaxManualSpeed = 0.3f; @@ -87,11 +110,14 @@ public class CartDefinition : CartActivator.CartDefinition [AsInitParam(desc = "遥控转向输入幂数")] public float ManualThetaPow = 2f; + [AsInitParam(desc = "遥控转向安全系数(相对奇异转角的比例,0~1)")] + public float ManualSteerSafetyRatio = 0.8f; + [AsInitParam(desc = "舵轮前后半轴距(mm)")] - public float WheelBaseHalfLength = 750f; + public float WheelBaseHalfLength = 525f; [AsInitParam(desc = "舵轮左右半轮距(mm)")] - public float WheelTrackHalfWidth = 500f; + public float WheelTrackHalfWidth = 200f; [AsInitParam(desc = "舵轮最小角度(deg)")] public float WheelAngleLowerLimit = -120f; diff --git a/MultiWheel/MultiWheelM/FleetRemote.cs b/MultiWheel/MultiWheelM/FleetRemote.cs new file mode 100644 index 0000000..6c84e25 --- /dev/null +++ b/MultiWheel/MultiWheelM/FleetRemote.cs @@ -0,0 +1,183 @@ +using System; +using System.IO; +using CartActivator; +using CycleGUI; +using CycleGUI.API; +using Medulla.Types; + +namespace MultiWheelM; + +public partial class CartDefinition +{ + private bool _fleetRemoteActive; + private DateTime _fleetDiagLastStick = DateTime.MinValue; + + // 写盘诊断日志（Medulla 侧）。文件落在 Medulla 工作目录，便于复现后离线分析。 + // 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。 + private static bool _fleetDiagInit; + private void FleetDiag(string msg) + { + try + { + const string path = "fleet_diag_medulla.log"; + var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}"; + if (!_fleetDiagInit) + { + _fleetDiagInit = true; + File.WriteAllText(path, + $"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line); + } + else + File.AppendAllText(path, line); + } + catch + { + // 诊断日志失败不影响主流程 + } + } + + private void ZeroFleetManualFields() + { + MultiVehicleManualEnabled = false; + MultiVehicleManualMode = 0; + MultiVehicleManualVx = 0; + MultiVehicleManualVy = 0; + MultiVehicleManualVth = 0; + } + + [IOObjectUtility] + public void FleetRemote() + { + if (_fleetRemoteActive) + { + GUI.GetPanelInstancingObject(this)?.BringToFront(); + return; + } + + _fleetRemoteActive = true; + var fleetOn = false; + // 速度比例 0~1，作用于摇杆输出的线速度与转向角。 + var speedRatio = 1f; + UseGesture? manip = null; + + void CleanupFleetRemote() + { + manip?.End(); + manip = null; + fleetOn = false; + ZeroFleetManualFields(); + _fleetRemoteActive = false; + } + + manip = new UseGesture(); + + manip.AddWidget(new UseGesture.ToggleWidget + { + name = "fleet_enable", + text = "车队联动", + position = "12.5%+10px, 75%+10px", + size = "25%-10px, 12.5%-10px", + OnValue = b => + { + fleetOn = b; + MultiVehicleManualEnabled = b; + MultiVehicleManualMode = 0; + if (!b) + ZeroFleetManualFields(); + FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled}"); + } + }); + + manip.AddWidget(new UseGesture.ThrottleWidget + { + name = "fleet_speed_ratio", + text = "速度比例", + position = "50%+10px, 75%+10px", + size = "50%-10px, 12.5%-10px", + bounceBack = false, + OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1) + }); + + manip.AddWidget(new UseGesture.ButtonWidget + { + name = "fleet_stop", + text = "急停/归零", + position = "25%+10px, 87.5%+10px", + size = "25%-10px, 12.5%-10px", + OnPressed = _ => + { + MultiVehicleManualVx = 0; + MultiVehicleManualVy = 0; + MultiVehicleManualVth = 0; + FleetDiag("BUTTON stop/zero"); + } + }); + + manip.AddWidget(new UseGesture.StickWidget + { + name = "fleet_stick", + text = "速度摇杆", + position = "37.5%+10px, 25%+10px", + size = "37.5%-10px, 37.5%-10px", + bounceBack = true, + keyboard = "Up,Down,Left,Right", + joystick = "Axis0,Axis1", + OnValue = (pos, manipulating) => + { + if (!fleetOn) + { + MultiVehicleManualVx = 0; + MultiVehicleManualVy = 0; + MultiVehicleManualVth = 0; + if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200) + { + _fleetDiagLastStick = DateTime.Now; + FleetDiag($"STICK(ignored,fleetOff) pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating}"); + } + return; + } + + // pos.Y(前后) → 车队线速度(m/s)；pos.X(左右) → 车队转向角(deg)。松手 bounceBack 自动回零。 + var ratio = Math.Clamp(speedRatio, 0, 1); + MultiVehicleManualVx = Math.Clamp(pos.Y, -1, 1) * MaxManualSpeed * ratio; + MultiVehicleManualVy = 0; + MultiVehicleManualVth = Math.Clamp(pos.X, -1, 1) * MaxManualAngularSpeed * ratio; + + if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200) + { + _fleetDiagLastStick = DateTime.Now; + FleetDiag($"STICK pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " + + $"maxV={MaxManualSpeed:0.00} maxW={MaxManualAngularSpeed:0.0} " + + $"-> Vx={MultiVehicleManualVx:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}"); + } + } + }); + + manip.ChangeState(new SetAppearance { drawGuizmo = false }); + manip.Start(); + + GUI.PromptOrBringToFront(pb => + { + if (pb.Closing()) + { + CleanupFleetRemote(); + pb.Panel.Exit(); + return; + } + + pb.Panel.TopMost(true) + .SetDefaultDocking(Panel.Docking.None) + .ShowTitle("车队联动遥控") + .InitSize(320, 160) + .InitPos(false, -32, 32, 1, 0, 1, 0); + + pb.SeparatorText("状态"); + pb.Label($"车队联动: {MultiVehicleManualEnabled}"); + pb.Label($"速度比例: {speedRatio:0.00}"); + pb.Label($"Vx={MultiVehicleManualVx:0.000} m/s, Vth={MultiVehicleManualVth:0.0} deg/s"); + pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})"); + pb.Label("请勿同时打开「手动控制」面板"); + pb.Panel.Repaint(); + }, instancingObject: this); + } +} diff --git a/MultiWheel/MultiWheelM/MotorRoutine.cs b/MultiWheel/MultiWheelM/MotorRoutine.cs new file mode 100644 index 0000000..de5d6a8 --- /dev/null +++ b/MultiWheel/MultiWheelM/MotorRoutine.cs @@ -0,0 +1,18 @@ +using CartActivator; + +namespace MultiWheelM; + +public class MotorRoutine : LadderLogic +{ + public override void Operation(int iteration) + { + cart.ActualSpeedLeftFront = cart.SpeedLeftFront; + cart.ActualSpeedLeftRear = cart.SpeedLeftRear; + cart.ActualSpeedRightFront = cart.SpeedRightFront; + cart.ActualSpeedRightRear = cart.SpeedRightRear; + cart.ActualThLeftFront = cart.ThLeftFront; + cart.ActualThLeftRear = cart.ThLeftRear; + cart.ActualThRightFront = cart.ThRightFront; + cart.ActualThRightRear = cart.ThRightRear; + } +} diff --git a/DiffWheelM/DiffWheelM.csproj b/MultiWheel/MultiWheelM/MultiWheelM.csproj similarity index 54% rename from DiffWheelM/DiffWheelM.csproj rename to MultiWheel/MultiWheelM/MultiWheelM.csproj index ae96fc9..3c40548 100644 --- a/DiffWheelM/DiffWheelM.csproj +++ b/MultiWheel/MultiWheelM/MultiWheelM.csproj @@ -6,24 +6,31 @@ enable AnyCPU false - ..\build\Medulla\plugins\ + ..\..\build\Medulla\plugins\ - D:\MDCS\Dependencies\Medulla\RefCartActivator.dll + D:\MDCS\Release\Medulla\RefCartActivator.dll False - D:\MDCS\Dependencies\Medulla\RefMedullaCore.dll + D:\MDCS\Release\Medulla\RefMedullaCore.dll False - D:\MDCS\Dependencies\Medulla\RefMedulla2.dll + D:\MDCS\Release\Medulla\RefMedulla2.dll False - D:\MDCS\Dependencies\deps\RefFundamentalLib.dll + D:\MDCS\Release\deps\RefFundamentalLib.dll + False + + + D:\MDCS\Release\CommonUsage.dll + + + D:\MDCS\Release\CycleGUI\RefCycleGUI.dll False diff --git a/MultiWheelM/Remote.cs b/MultiWheel/MultiWheelM/Remote.cs similarity index 68% rename from MultiWheelM/Remote.cs rename to MultiWheel/MultiWheelM/Remote.cs index dae9790..fa20068 100644 --- a/MultiWheelM/Remote.cs +++ b/MultiWheel/MultiWheelM/Remote.cs @@ -7,29 +7,35 @@ public class Remote : ManualController [AsControlItem(name = "速度摇杆", keyboard = "Up,Down,Left,Right", joystick = "Axis0,Axis1")] public Stick SpeedPad; - [AsControlItem(name = "速度比例", keyboard = "Q,W")] + [AsControlItem(name = "速度比例")] public Throttle SpeedRatio; - [AsControlItem(name = "旋转模式", keyboard = "R")] + [AsControlItem(name = "旋转模式")] public Switch SpinMode; - [AsControlItem(name = "横移模式", keyboard = "C")] + [AsControlItem(name = "横移模式")] public Switch CrabMode; - [AsControlItem(name = "斜行模式", keyboard = "S")] + [AsControlItem(name = "斜行模式")] public Switch SwayMode; - [AsControlItem(name = "车头方向", keyboard = "A,D")] + [AsControlItem(name = "车头方向")] public DThrottle FrontDirection; public override void Operation() { + cart.MultiVehicleManualEnabled = false; + cart.MultiVehicleManualVx = cart.MultiVehicleManualVy = cart.MultiVehicleManualVth = 0; + var speedThreshold = Math.Clamp(SpeedRatio.val, 0, 1) * Math.Max(0, cart.MaxManualSpeed); var speed = Math.Clamp(SpeedPad.y, -1, 1) * speedThreshold; var steerInput = Math.Clamp(SpeedPad.x, -1, 1); var steering = (float)Math.Pow(Math.Abs(steerInput), cart.ManualThetaPow) * Math.Sign(steerInput); - var frontTh = -steering * cart.MaxManualTheta; - var rearTh = steering * cart.MaxManualTheta; + + var steerLimit = ComputeSafeSteerLimit(); + var axleTheta = Math.Clamp(steering * cart.MaxManualTheta, -steerLimit, steerLimit); + var frontTh = -axleTheta; + var rearTh = axleTheta; var frontDirection = Math.Clamp(FrontDirection.dval, -1, 1) * 180; if (SpinMode.on) @@ -64,7 +70,13 @@ public class Remote : ManualController cart.ManualMode = 0; statusText = $"normal, v={speed:0.000}, front={frontTh:0.0}, rear={rearTh:0.0}"; } + + private float ComputeSafeSteerLimit() + { + var radius = Math.Max(1f, cart.ChassisControlPointRadius); + var halfTrack = Math.Max(1f, cart.WheelTrackHalfWidth); + var singularTheta = (float)(Math.Atan2(radius, halfTrack) * 180.0 / Math.PI); + var ratio = Math.Clamp(cart.ManualSteerSafetyRatio, 0.1f, 1f); + return Math.Min(cart.MaxManualTheta, singularTheta * ratio); + } } - - - diff --git a/MultiWheelM/MotorRoutine.cs b/MultiWheelM/MotorRoutine.cs deleted file mode 100644 index 4ffd363..0000000 --- a/MultiWheelM/MotorRoutine.cs +++ /dev/null @@ -1,11 +0,0 @@ -using CartActivator; - -namespace MultiWheelM; - -public class MotorRoutine : LadderLogic -{ - public override void Operation(int iteration) - { - - } -} diff --git a/README.md b/README.md index b67eb0b..49ee956 100644 --- a/README.md +++ b/README.md @@ -1,72 +1,75 @@ -# Tutorial Medulla Plugins +# Tutorial Medulla / Clumsy 插件 -本仓库用于放置最小 Medulla 插件示例，方便从产品化项目中抽离出底盘 IO 与遥控器逻辑。示例不引用 `MDCSToolBox`；其中 `MultiWheelM` 为了使用底盘几何模型，会额外引用 `CommonUsage`。 +本仓库提供 **差速** 与 **多舵轮** 两套最小可用示例，每种底盘各含 Medulla (M) 与 Clumsy (C) 插件。多舵轮示例包含 **双车联动（MultiVehicleSync）** 的手动与自动轨迹跟踪演示。 ## 目录结构 ```text Tutorial/ Tutorial.sln - DiffWheelM/ - 差速底盘最小 Medulla 插件 - MultiWheelM/ - 多舵轮底盘最小 Medulla 插件 -``` - -`build/`、`obj/` 和 `.vs/` 是本地构建或调试输出，已在 `.gitignore` 中忽略。 - -## DiffWheelM - -`DiffWheelM` 参考 `StandardKivaM`，保留差速底盘最小 IO： - -- `SpeedLeft` / `SpeedRight`: 左右轮下发速度，`AsUpperIO`，超时自动复位 -- `ActualSpeedLeft` / `ActualSpeedRight`: 左右轮实际速度反馈，`AsLowerIO` -- `MaxManualSpeed`: 手动最大轮速初始化参数，默认 `0.3m/s` - -遥控器使用 `Up,Down,Left,Right` 控制前后与转向，`Q,W` 控制速度比例。 - -## MultiWheelM - -`MultiWheelM` 参考 `StandardMultiWheelLifterM`，保留多舵轮底盘最小 IO： - -- 四个舵轮速度：`SpeedLeftFront`、`SpeedLeftRear`、`SpeedRightFront`、`SpeedRightRear` -- 四个舵轮转角：`ThLeftFront`、`ThLeftRear`、`ThRightFront`、`ThRightRear` -- 对应的实际速度与实际转角反馈 -- `MaxManualSpeed`、`MaxManualAngularSpeed`、`MaxManualTheta` 初始化参数 -- 舵轮几何参数：`WheelBaseHalfLength`、`WheelTrackHalfWidth`、`WheelAngleLowerLimit`、`WheelAngleUpperLimit` - -遥控器支持普通转向、旋转、横移和斜行几个最小模式： - -- `Up,Down,Left,Right`: 速度摇杆 -- `Q,W`: 速度比例 -- `R`: 旋转模式 -- `C`: 横移模式 -- `S`: 斜行模式 -- `A,D`: 车头方向偏置 - -当前多舵轮示例使用 `CommonUsage.Chassis.MultiWheelChassis` 做遥控几何分解，不接真实下位机，`MotorRoutine` 会把下发速度/角度镜像到实际反馈变量，便于 Medulla UI 或 Clumsy 侧观察。 - -## 构建 - -```powershell -dotnet build .\Tutorial.sln -``` - -默认输出目录： - -```text -build\Medulla\plugins\ + DiffWheel/ + DiffWheelM/ 差速 Medulla 插件 + DiffWheelC/ 差速 Clumsy 插件 + MultiWheel/ + MultiWheelM/ 多舵轮 Medulla 插件 + MultiWheelC/ 多舵轮 Clumsy 插件（含双车联动） + deploy/ Clumsy/Medulla 配置模板 + docs/ 算法原理、修复记录、部署说明 + build/ 本地运行目录（gitignore） ``` ## 依赖 -两个插件都只引用： +统一引用 `D:\MDCS\Release`： -- `D:\MDCS\Dependencies\Medulla\RefCartActivator.dll` -- `D:\MDCS\Dependencies\Medulla\RefMedullaCore.dll` -- `D:\MDCS\Dependencies\Medulla\RefMedulla2.dll` -- `D:\MDCS\Dependencies\deps\RefFundamentalLib.dll` +- `Release\Medulla\RefCartActivator.dll` 等 +- `Release\Clumsy\RefClumsyCore.dll`、`RefClumsyDance.dll` +- `Release\CommonUsage.dll` +- `Release\MDCSToolBox.dll`（需先编译 `D:\MDCS\Source\Products\mdcstoolbox`） -`MultiWheelM` 额外引用： +```powershell +dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release +dotnet build .\Tutorial.sln +``` -- `D:\MDCS\Dependencies\Commons\CommonUsage.dll` +默认输出： + +- M 插件 → `build\Medulla\plugins\` +- C 插件 → `build\Clumsy\`、`build\Clumsy_AGV2\`（多舵轮） +- 差速 C 插件 → `build\Clumsy_diff\` + +## DiffWheelM / DiffWheelC + +差速底盘最小 IO： + +- `SpeedLeft` / `SpeedRight`：下发速度 +- `ActualSpeedLeft` / `ActualSpeedRight`：反馈（M 侧 MotorRoutine 镜像） + +遥控器：`Up,Down,Left,Right` + `Q,W` 速度比例。 + +Clumsy 侧提供 **差速-前进2秒**、**差速-原地旋转2秒** MovementTest。 + +## MultiWheelM / MultiWheelC + +多舵轮底盘 IO 与 `CommonUsage.Chassis.MultiWheelChassis` 几何一致。M 侧 `MotorRoutine` 将下发速度/角度镜像到反馈，便于仿真观察。 + +遥控器模式：`R` 旋转、`C` 横移、`S` 斜行、`A,D` 车头偏置、**`F` 车队联动**。 + +### 双车联动 + +- AGV1：`build\Medulla`（tag `Multi1`）+ `build\Clumsy`（主车，`MultiVehicleMasterIp="/"`） +- AGV2：`build\Medulla_AGV2`（tag `Multi2`）+ `build\Clumsy_AGV2`（从车） + +主从通过 HTTP `:8008` 同步；详见 [docs/MultiVehicleSync.md](docs/MultiVehicleSync.md)。 + +产品源已知问题及 Tutorial 修复说明：[docs/BugFixes.md](docs/BugFixes.md)。 + +运行步骤：[docs/RunAndDeploy.md](docs/RunAndDeploy.md)。 + +## M 与 C 如何通信 + +两者不直接调用，而是通过 **CartActivator DObject 共享内存**（相同 IO 字段名 + `SetShareObjectTag`）。Clumsy 写轮速/舵角 UpperIO，Medulla 读并下发；Medulla 写反馈 LowerIO，Clumsy 读用于 ghost/控制。 + +## 构建与忽略 + +`build/`、`obj/`、`.vs/` 已在 `.gitignore` 中忽略。配置模板在 `deploy/`，首次运行 Clumsy 时可复制到 `build\Clumsy*`。 diff --git a/Tutorial.sln b/Tutorial.sln index 6f902f5..8e0fe84 100644 --- a/Tutorial.sln +++ b/Tutorial.sln @@ -3,9 +3,13 @@ Microsoft Visual Studio Solution File, Format Version 12.00 # Visual Studio Version 17 VisualStudioVersion = 17.5.33424.131 MinimumVisualStudioVersion = 10.0.40219.1 -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DiffWheelM", "DiffWheelM\DiffWheelM.csproj", "{7C6F23A7-2B4D-45C2-98E9-6C55FBB0F1B8}" +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DiffWheelM", "DiffWheel\DiffWheelM\DiffWheelM.csproj", "{7C6F23A7-2B4D-45C2-98E9-6C55FBB0F1B8}" EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelM", "MultiWheelM\MultiWheelM.csproj", "{B7B53B6B-98B5-4B6F-9B09-598F7B8166C9}" +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DiffWheelC", "DiffWheel\DiffWheelC\DiffWheelC.csproj", "{A1B2C3D4-E5F6-7890-ABCD-EF1234567890}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelM", "MultiWheel\MultiWheelM\MultiWheelM.csproj", "{B7B53B6B-98B5-4B6F-9B09-598F7B8166C9}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelC", "MultiWheel\MultiWheelC\MultiWheelC.csproj", "{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution @@ -17,10 +21,18 @@ Global {7C6F23A7-2B4D-45C2-98E9-6C55FBB0F1B8}.Debug|Any CPU.Build.0 = Debug|Any CPU {7C6F23A7-2B4D-45C2-98E9-6C55FBB0F1B8}.Release|Any CPU.ActiveCfg = Release|Any CPU {7C6F23A7-2B4D-45C2-98E9-6C55FBB0F1B8}.Release|Any CPU.Build.0 = Release|Any CPU + {A1B2C3D4-E5F6-7890-ABCD-EF1234567890}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {A1B2C3D4-E5F6-7890-ABCD-EF1234567890}.Debug|Any CPU.Build.0 = Debug|Any CPU + {A1B2C3D4-E5F6-7890-ABCD-EF1234567890}.Release|Any CPU.ActiveCfg = Release|Any CPU + {A1B2C3D4-E5F6-7890-ABCD-EF1234567890}.Release|Any CPU.Build.0 = Release|Any CPU {B7B53B6B-98B5-4B6F-9B09-598F7B8166C9}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {B7B53B6B-98B5-4B6F-9B09-598F7B8166C9}.Debug|Any CPU.Build.0 = Debug|Any CPU {B7B53B6B-98B5-4B6F-9B09-598F7B8166C9}.Release|Any CPU.ActiveCfg = Release|Any CPU {B7B53B6B-98B5-4B6F-9B09-598F7B8166C9}.Release|Any CPU.Build.0 = Release|Any CPU + {C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Debug|Any CPU.Build.0 = Debug|Any CPU + {C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.ActiveCfg = Release|Any CPU + {C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/deploy/chassis.json b/deploy/chassis.json new file mode 100644 index 0000000..a6d0081 --- /dev/null +++ b/deploy/chassis.json @@ -0,0 +1,15 @@ +{ + "WheelConfig": { + "LeftFront": { "Position": { "X": 525.0, "Y": 200.0 } }, + "LeftRear": { "Position": { "X": -525.0, "Y": 200.0 } }, + "RightFront": { "Position": { "X": 525.0, "Y": -200.0 } }, + "RightRear": { "Position": { "X": -525.0, "Y": -200.0 } } + }, + "MaxSpeed": 0.3, + "AccPerSecond": 0.3, + "DeAccPerSecond": 0.5, + "MinTurnSpeedFac": 0.25, + "ControlPointRadius": 500.0, + "GcpThetaPerSecond": 10.0, + "MinimumTurningAngleForAckermann": 60.0 +} diff --git a/deploy/clumsy_agv1/clumsy.json b/deploy/clumsy_agv1/clumsy.json new file mode 100644 index 0000000..ad933e7 --- /dev/null +++ b/deploy/clumsy_agv1/clumsy.json @@ -0,0 +1,60 @@ +{ + "basicSpeed": 0.2, + "DriveTaskInterval": 50, + "script": "MultiWheelC.dll", + "detourHost": "127.0.0.1", + "detourPort": 4321, + "msConf": { + "MultiVehicleMasterEndpoint": "/", + "MultiVehicleFleetNum": 2, + "MultiVehicleSyncInterval": 50, + "TestCarSyncDistance": 2400, + "TestCarSyncTh": 0, + "ManualCarSyncVxFac": 1.0, + "ManualCarSyncVthFac": 1.0, + "SyncThAccPerSec": 30, + "PosAvailable": true, + "SimpleIp": "127.0.0.1", + "CarNum": 1 + }, + "layout": { + "chassis": { + "width": 1000, + "length": 1550, + "contour": [ + 750, -500, -750, -500, -750, -300, -800, -280, -800, -220, + -750, -200, -750, 200, -800, 220, -800, 280, -750, 300, + -750, 500, 750, 500 + ] + }, + "components": [ + { + "type": "lidar2d", + "options": { + "name": "front_lidar_1", + "x": 700, "y": 0, "yaw": 0, "z": 0, "pitch": 0, "roll": 0, + "isCircle": false, "ignoreDist": 100, "maxDist": 200000, + "filterChassis": true + } + }, + { + "type": "lidar2d", + "options": { + "name": "rear_left_lidar_1", + "x": -700, "y": 450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0, + "isCircle": false, "ignoreDist": 100, "maxDist": 200000, + "filterChassis": true + } + }, + { + "type": "lidar2d", + "options": { + "name": "rear_right_lidar_1", + "x": -700, "y": -450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0, + "isCircle": false, "ignoreDist": 100, "maxDist": 200000, + "filterChassis": true + } + } + ] + } +} diff --git a/deploy/clumsy_agv1/clumsyconsole.json b/deploy/clumsy_agv1/clumsyconsole.json new file mode 100644 index 0000000..4c8da7e --- /dev/null +++ b/deploy/clumsy_agv1/clumsyconsole.json @@ -0,0 +1,11 @@ +{ + "port": 8008, + "allowMultiple": true, + "soTag": "Multi1", + "detourHost": "127.0.0.1", + "detourPort": 4321, + "HideConsoleOnStart": false, + "FollowCarOnStart": true, + "ShowRobot3dModel": true, + "ShowRobotArrow": true +} diff --git a/deploy/clumsy_agv2/clumsy.json b/deploy/clumsy_agv2/clumsy.json new file mode 100644 index 0000000..33de771 --- /dev/null +++ b/deploy/clumsy_agv2/clumsy.json @@ -0,0 +1,60 @@ +{ + "basicSpeed": 0.2, + "DriveTaskInterval": 50, + "script": "MultiWheelC.dll", + "detourHost": "127.0.0.1", + "detourPort": 4421, + "msConf": { + "MultiVehicleMasterEndpoint": "127.0.0.1:8008", + "MultiVehicleFleetNum": 2, + "MultiVehicleSyncInterval": 50, + "TestCarSyncDistance": 2400, + "TestCarSyncTh": 0, + "ManualCarSyncVxFac": 1.0, + "ManualCarSyncVthFac": 1.0, + "SyncThAccPerSec": 30, + "PosAvailable": true, + "SimpleIp": "127.0.0.1", + "CarNum": 2 + }, + "layout": { + "chassis": { + "width": 1000, + "length": 1550, + "contour": [ + 750, -500, -750, -500, -750, -300, -800, -280, -800, -220, + -750, -200, -750, 200, -800, 220, -800, 280, -750, 300, + -750, 500, 750, 500 + ] + }, + "components": [ + { + "type": "lidar2d", + "options": { + "name": "front_lidar_2", + "x": 700, "y": 0, "yaw": 0, "z": 0, "pitch": 0, "roll": 0, + "isCircle": false, "ignoreDist": 100, "maxDist": 200000, + "filterChassis": true + } + }, + { + "type": "lidar2d", + "options": { + "name": "rear_left_lidar_2", + "x": -700, "y": 450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0, + "isCircle": false, "ignoreDist": 100, "maxDist": 200000, + "filterChassis": true + } + }, + { + "type": "lidar2d", + "options": { + "name": "rear_right_lidar_2", + "x": -700, "y": -450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0, + "isCircle": false, "ignoreDist": 100, "maxDist": 200000, + "filterChassis": true + } + } + ] + } +} diff --git a/deploy/clumsy_agv2/clumsyconsole.json b/deploy/clumsy_agv2/clumsyconsole.json new file mode 100644 index 0000000..0cef8f2 --- /dev/null +++ b/deploy/clumsy_agv2/clumsyconsole.json @@ -0,0 +1,11 @@ +{ + "port": 8009, + "allowMultiple": true, + "soTag": "Multi2", + "detourHost": "127.0.0.1", + "detourPort": 4421, + "HideConsoleOnStart": false, + "FollowCarOnStart": true, + "ShowRobot3dModel": true, + "ShowRobotArrow": true +} diff --git a/deploy/clumsy_diff/clumsy.json b/deploy/clumsy_diff/clumsy.json new file mode 100644 index 0000000..0f6235a --- /dev/null +++ b/deploy/clumsy_diff/clumsy.json @@ -0,0 +1,17 @@ +{ + "basicSpeed": 0.2, + "DriveTaskInterval": 50, + "script": "DiffWheelC.dll", + "msConf": { + "ChassisWidth": 600, + "SimpleIp": "127.0.0.1" + }, + "layout": { + "chassis": { + "width": 600, + "length": 900, + "contour": [-450, 300, 450, 300, 450, -300, -450, -300] + }, + "components": [] + } +} diff --git a/deploy/start_clumsy_agv1.bat b/deploy/start_clumsy_agv1.bat new file mode 100644 index 0000000..d0a8054 --- /dev/null +++ b/deploy/start_clumsy_agv1.bat @@ -0,0 +1,9 @@ +@echo off +setlocal +cd /d "%~dp0" +if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul +if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv1\clumsy.json clumsy.json >nul +if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv1\clumsyconsole.json clumsyconsole.json >nul +echo [Clumsy AGV1] Master, tag Multi1. Start Medulla build\Medulla first. +ClumsyLite.exe +pause diff --git a/deploy/start_clumsy_agv2.bat b/deploy/start_clumsy_agv2.bat new file mode 100644 index 0000000..856875e --- /dev/null +++ b/deploy/start_clumsy_agv2.bat @@ -0,0 +1,9 @@ +@echo off +setlocal +cd /d "%~dp0" +if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul +if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv2\clumsy.json clumsy.json >nul +if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv2\clumsyconsole.json clumsyconsole.json >nul +echo [Clumsy AGV2] Slave, tag Multi2. Start Medulla build\Medulla_AGV2 first. +ClumsyLite.exe +pause diff --git a/docs/BugFixes.md b/docs/BugFixes.md new file mode 100644 index 0000000..1da2f10 --- /dev/null +++ b/docs/BugFixes.md @@ -0,0 +1,129 @@ +# 多车联动问题修复记录 + +对照产品源 `StandardMultiWheelLifterC/PilotDefinition.cs`，Tutorial `MultiWheelC` 中已修复或规避的问题如下。 + +## 1. CenterTh 弧度/角度混用 + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L534–536 | +| **问题** | `Math.Sin(v.Th)` / `Math.Cos(v.Th)` 中 `v.Th` 为角度（deg），但 `Sin/Cos` 需要弧度，导致车队中心航向错误 | +| **影响** | 自动/定位补偿模式下 fleet 中心朝向偏差，联动走形异常 | +| **修复** | 改为 `Math.Sin(v.Th * Math.PI / 180.0)` | + +## 2. 空 fleet 上调用 Average() + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L531–535 | +| **问题** | 主车在把自己写入 fleet **之前**对 `MultiVehicleFleet.Values` 求平均，早期 tick 可能为空 | +| **影响** | 可能抛 `InvalidOperationException`，sync 线程崩溃 | +| **修复** | 先写入主车自身，再 `Count > 0` 时才 Average | + +## 3. 阻塞式 HTTP (.Result) + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L484, L778, L802 | +| **问题** | 50ms 循环内 `hc.GetStringAsync(...).Result` 阻塞，超时 2s | +| **影响** | sync 线程卡顿，motion 与 notify 延迟累积 | +| **修复** | 改为 `FireAndForgetRegister/Notify`，`ContinueWith` 记录失败 | + +## 4. if(true)/if(false) 死分支 + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L674–679 | +| **问题** | fleet 数量门控与预对齐阶段被硬编码关闭 | +| **影响** | 未齐套也 SendMotion；对齐逻辑永不执行 | +| **修复** | 恢复 `fleetReady = Count == MultiVehicleFleetNum` 门控后再 SendMotion | + +## 5. _accumulateTh 积分 dt 异常 + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L547–550 | +| **问题** | `_lastThTime` 初值为 `MinValue` 或 >1s 重置导致首帧/恢复后 dt 异常 | +| **影响** | 手动编队转向突变或几乎不动 | +| **修复** | 初始化 `_lastThTime = Now`，dt 上限 0.2s | + +## 6. 主车 HTTP 通知自身 + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L770–778 | +| **问题** | notify 循环包含主车自己的 IP | +| **影响** | 冗余自调用、handler 重入 | +| **修复** | `ip == Conf.SimpleIp` 时 skip | + +## 7. 从车覆盖全局 Conf + +| 项 | 说明 | +|----|------| +| **位置** | 产品源约 L585–596 | +| **问题** | 从 notification 写回 `Conf.TestCarSyncTh` 等全局配置 | +| **影响** | 网络数据污染本地配置，难以调试 | +| **修复** | 使用局部变量 `syncTh/syncDistance/...`，不修改 `Conf` | + +## 8. ChassisController API 名称/签名错误 + +| 项 | 说明 | +|----|------| +| **位置** | 产品源 `ChassisController.cs` L95–117 | +| **问题** | 使用 `MultiVehicleGetCenterPos`（不存在）及 3 参 `MultiVehicleSendMotion` | +| **影响** | 自动联动可能无法编译或静默失效 | +| **修复** | 使用 `MultiVehicleGetFleetPos` + 5 参 `Action` | + +## 9. VehicleSyncInfo JSON 反序列化 + +| 项 | 说明 | +|----|------| +| **位置** | 产品源 DTO 定义 | +| **问题** | 只读字段 + 有参构造，Newtonsoft 默认难反序列化 | +| **影响** | register/notify 解析失败 → HTTP 500 | +| **修复** | 改为可写属性 + `[JsonProperty]`（见 `VehicleSyncModels.cs`） | + +## 10. 手动联动未接线（产品层） + +| 项 | 说明 | +|----|------| +| **位置** | `StandardMultiWheelLifterM/Remote.cs`，`MultiWheelRemote.MultiVehicleModeChassisLogic` 为空 | +| **问题** | `MultiVehicleManual*` LowerIO 从未被 M 插件赋值 | +| **影响** | 遥控器无法驱动 fleet | +| **修复** | Tutorial `MultiWheelM`：`FleetMode` 开关 + `MultiVehicleManual*` UpperIO 字段 | + +## 11. MultiVehicleManualVy 未使用（产品源） + +| 项 | 说明 | +|----|------| +| **位置** | 产品源声明但未读 | +| **问题** | 横移联动未实现 | +| **Tutorial 状态** | 保留字段，当前手动模式仅 Vx + Vth；Vy 固定为 0 | + +## 12. 产品源仍存在的已知限制（Tutorial 未改框架） + +- ~~激光两腿检测联动~~ → 已上移到 MDCSToolBox 接口；Tutorial 默认不启用检测，产品在插件内实现 `DetectNeighborBias` +- `ClumsyLite` 与 Medulla share-object tag 的绑定机制依赖运行时 CartActivator 配置，需与 `startup.iocmd` 中 `SetShareObjectTag` 一致 + +## 13. 同机多 Detour/Clumsy 串台（2026-06） + +| 项 | 说明 | +|----|------| +| **问题** | 写死 `DObject("DetourPos")`、Detour HTTP `127.0.0.1:4321`、同步 HTTP `:8008`，同 PC 双实例互相覆盖 | +| **修复** | Detour `shareObjectTag` + Clumsy `soTag`；Clumsy `detourHost`/`detourPort`；`WebAPI.port` + `VehicleSyncInfo.Port`；主车端点 `MultiVehicleMasterEndpoint`（`ip:port`） | +| **配置** | 见 `deploy/clumsy_agv*/clumsyconsole.json`、`detour.json` | + +## 14. 车队中心取平均（2026-06） + +| 项 | 说明 | +|----|------| +| **问题** | 旧逻辑对全部车位姿取平均，且假设每台都有 Detour | +| **修复** | 固定 slot 表 + 仅对 `PosAvailable` 车反推中心；有效车集合运行中可变 | +| **位置** | `MultiWheelPilotDefinition.MultiVehicle.cs` → `TryInferFleetCenter` | + +## 15. 同步引擎重复实现（2026-06） + +| 项 | 说明 | +|----|------| +| **问题** | Tutorial 与 `StandardMultiWheelLifterC` 各维护一套 register/notify/loop | +| **修复** | 统一到 MDCSToolBox 基类 `InitMultiVehicleCoordination()`；子类仅配置 + `DetectNeighborBias` 重写 | diff --git a/docs/MultiVehicleConfig.md b/docs/MultiVehicleConfig.md new file mode 100644 index 0000000..60709a0 --- /dev/null +++ b/docs/MultiVehicleConfig.md @@ -0,0 +1,166 @@ +# 多车联动 配置与参数清单 + +本文件汇总 **MultiWheel 多车联动** 需要调整的全部配置项，分三层： + +1. **每台车都不同**的身份/通信项（必须按车区分，配错直接导致组不成队或互相抢占）。 +2. **编队几何**项（决定车间距、布局、互识别 ROI，全队应一致）。 +3. **闭环/遥控/安全**调参项（手感与稳定性，可按需微调）。 + +涉及文件： + +| 端 | 文件 | 作用 | +|----|------|------| +| Medulla | `build/Medulla*/startup.iocmd` | 插件加载、车号 `CarNum`、共享内存 tag、Playground 绑定、雷达初始化 | +| Medulla | 车体「Fields」面板 / `loader setp <字段> <值>` | `CartDefinition` 中 `[AsInitParam]` 字段（车号、手动最大速度等） | +| Clumsy | `build/Clumsy*/clumsy.json` → `msConf` | `PilotConfig` 全部联动参数（间距、检测、补偿、遥控系数等） | +| Clumsy | `build/Clumsy*/clumsyconsole.json` | `port`、`soTag`、`detourPort`（通信端口/共享内存/定位） | + +> 提示：`msConf` 的 key 与 `MultiWheelC/PilotConfig.cs` 字段一一对应；Medulla 的 `[AsInitParam]` 字段见 `MultiWheelM/CartDefinition.cs`。 + +--- + +## 1. 每台车都不同（身份与通信） + +以 Playground 双车（车1=主、车2=从）为例，**逐项对照**： + +| 配置位置 | 字段 | 车1（主车） | 车2（从车） | 说明 | +|----------|------|-------------|-------------|------| +| Medulla `startup.iocmd` | `SetShareObjectTag` | `Multi1` | `Multi2` | 必须与对应 Clumsy 的 `soTag` 一致 | +| Medulla `startup.iocmd` | `loader setp CarNum` | `1` | `2` | 车号，决定编队 slot（±半间距）。**每台必须唯一** | +| Medulla `startup.iocmd` | `SetPlaygroundBinding` | `agv_multi_1` | `agv_multi_2` | 绑定的仿真小车名 | +| Medulla `startup.iocmd` | `lidar init ... ` | `agv_multi_1` | `agv_multi_2` | 雷达挂到对应小车 | +| Clumsy `clumsyconsole.json` | `port` | `8008` | `8009` | 本车 HTTP/同步端口 | +| Clumsy `clumsyconsole.json` | `soTag` | `Multi1` | `Multi2` | 与 Medulla 的 ShareObjectTag 一致 | +| Clumsy `clumsyconsole.json` | `detourPort` | `4321` | `4421` | 与对应 Detour 实例一致 | +| Clumsy `clumsy.json` | `MultiVehicleMasterEndpoint` | `/` | `127.0.0.1:8008` | `/`=本车为主车；从车填**主车的 ip:port** | +| Clumsy `clumsy.json` | `PlaygroundRobotName` | `agv_multi_1` | `agv_multi_2` | WebAPI 操作/查询的小车名 | +| Clumsy `clumsy.json` | `TwoLegLidarName` | `rear_left_lidar_1,rear_right_lidar_1` | `rear_left_lidar_2,rear_right_lidar_2` | 互识别用的后向雷达名（与本车 startup 中的雷达名一致） | +| Detour | 运行目录 / `detourPort` | `Detour` (4321) | `Detour_AGV2` (4421) | 两套独立定位 | + +**关键点** + +- `MultiVehicleMasterEndpoint`：只有主车为 `/`；从车必须指向主车端口（这里主车 Clumsy `port=8008`，故从车填 `127.0.0.1:8008`）。 +- `CarNum` 通过 Medulla 的 `[AsInitParam]` 配置（`setp` / Fields 面板 / `cartparams.json`），并以 `[AsLowerIO]` 上报给 Clumsy。**不要**改成 Clumsy 端写死。 +- 全队 `MultiVehicleFleetNum` 必须等于实际车数（这里=2），否则永远 `ready=false`，不下发运动。 + +--- + +## 2. 编队几何（全队一致） + +| 字段（`clumsy.json`） | 含义 | Playground 参考值 | 默认值 | +|------|------|------|--------| +| `TestCarSyncDistance` | 车队两端**中心到中心**间距(mm) | `2400` | 2400 | +| `TestCarSyncTh` | 编队布局方向偏角(deg)，0=沿 X 排布 | `0` | 0 | +| `DeltaDetectCenter` | 车中心到「互识别参考点」的偏移(mm) | `800` | 350 | +| `TwoLegGuessX` | 2腿检测初始猜测 X(车体系, mm) | `-1600` | 2000 | +| `MultiVehicleFleetNum` | 车队总数 | `2` | 2 | + +**几何来源（Playground 默认场景 `default_scene.json`）** + +- `agv_multi_1` 初始位 `x=20200`，`agv_multi_2` 初始位 `x=17800` → 中心间距 = **2400 mm**，朝向相差 180°。 +- 故 `TestCarSyncDistance = 2400`。 + +**必须遵守的换算关系** + +```text +互识别检测距离 detectDistance = TestCarSyncDistance - DeltaDetectCenter +TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) + = -(2400 - 800) = -1600 +``` + +- `TwoLegGuessX` 取**负号**：邻车在本车后方（用后向雷达 `rear_*`），猜测点在车体 -X 方向。 +- 改 `TestCarSyncDistance` 时，**务必同步**更新 `TwoLegGuessX`，否则检测 ROI 偏离 → 检测失败 → 安全门拦停。 +- `布局`（每车 slot）由 `TestCarSyncDistance`/`TestCarSyncTh` 唯一推导：车1=+半间距、车2=−半间距并翻转 180°。已删除独立的 `FormationSlots`，不要再引入重复参数。 + +--- + +## 3. 2 腿互识别（ROI / 聚类） + +与「多舵轮-2腿检测」MovementTest 共用同一套参数；调好 MovementTest 再用于联动。 + +| 字段（`clumsy.json`） | 含义 | Playground 参考值 | 默认值 | +|------|------|------|------| +| `TwoLegLidarName` | 后向雷达名（逗号分隔，按车区分，见 §1） | `rear_left_lidar_X,rear_right_lidar_X` | — | +| `TwoLegGuessX` | 初始猜测 X（见 §2 换算） | `-1600` | 2000 | +| `TwoLegWidth` | 两腿间距(mm) | `473` | 800 | +| `TwoLegWidthErr` | 两腿间距允许误差(mm) | `100` | 100 | +| `TwoLegFilterLength` | ROI 滤波框长(mm) | `500` | 1800 | +| `TwoLegFilterWidth` | ROI 滤波框宽(mm) | `1000` | 600 | +| `TwoLegBlobDist` / `TwoLegBlobSize` / `TwoLegBlobPtCount` | 聚类间距/尺寸/最小点数 | `100/200/5` | 100/200/5 | +| `TwoLegSgnDir` | 方向符号(±1) | `1` | 1 | +| `TwoLegCenterChangeX` | 中心 X 微调(mm) | `0` | 0 | + +--- + +## 4. 手动遥控（Medulla 车队遥控） + +遥控在 Medulla 侧产生指令（`MultiVehicleManual*`，`[AsLowerIO]` 上报 Clumsy）。摇杆输出已是物理单位，Clumsy 端系数应保持 **1（直通）**。 + +| 端 | 字段 | 含义 | 推荐值 | +|----|------|------|--------| +| Medulla `[AsInitParam]` | `MaxManualSpeed` | 车队手动**最大线速度(m/s)** | `0.3` | +| Medulla `[AsInitParam]` | `MaxManualAngularSpeed` | 车队手动**最大转向角(deg)** | `45` | +| Clumsy `clumsy.json` | `ManualCarSyncVxFac` | 手动 Vx 系数（**保持 1，勿再缩放**） | `1.0` | +| Clumsy `clumsy.json` | `ManualCarSyncVthFac` | 手动 Vth 系数（**保持 1**） | `1.0` | +| Clumsy `clumsy.json` | `SyncThAccPerSec` | 转向角爬升速率(deg/s) | `30` | + +> 历史坑：`ManualCarSyncVxFac=0.2 × MaxManualSpeed=0.3 → 0.06 m/s`，肉眼几乎不动；`VthFac=5 → 225°` 超舵轮范围。已统一为系数=1。 + +操作：在 Medulla 打开车体工具 **「FleetRemote / 车队联动遥控」**（workspace 摇杆，松手自动回零），开「车队联动」开关后拖摇杆即可。主车摇杆驱动全队；从车由主车广播自动跟随，**无需**各自开开关。**不要**同时打开普通「手动控制」面板（会抢占优先级）。 + +--- + +## 5. 闭环补偿与安全门 + +| 字段（`clumsy.json`） | 含义 | 推荐值 | +|------|------|--------| +| `MultiVehicleUseDetect` | 启用互识别纠正 **+ 安全门**（任一车检测不到邻车→整队停车） | `true` | +| `MultiVehicleDetectBiasXFac/YFac/ThFac` | 互识别补偿系数 | `0.5/0.5/0.5` | +| `MultiVehicleDetectBiasXThreshold/YThreshold/ThThreshold` | 互识别补偿上限(mm/mm/deg) | `50/50/5` | +| `MultiVehiclePosBiasXFac/YFac/ThFac` | SLAM 编队保持补偿系数 | `0.5/0.5/0.5` | +| `MultiVehiclePosBiasXThreshold/YThreshold/ThThreshold` | SLAM 补偿上限(mm/mm/deg) | `50/50/5` | +| `SingleCarSyncPrecisionXy` / `SingleCarSyncPrecisionTh` | 对齐精度 / 补偿死区(mm/deg) | `10 / 0.2` | +| `PosAvailable` | 是否启用 Detour 定位 | `true` | + +**要点** + +- `MultiVehicleUseDetect=true` 时若 2 腿检测没锁定，`fleetVx` 会被安全门置零（表现为摇杆"无效"——这是预期安全行为，不是 bug）。先确保检测稳定。 +- SLAM 补偿（`PosBias*`）要求两车**共享同一 SLAM 世界系**；否则编队中心反推会错。Playground 两车同图，满足。 +- 不需要绝对编队保持时，可把 `MultiVehiclePosBias*Fac` 设 0，仅靠互识别维持间距。 + +--- + +## 6. 常见坑位（排查清单） + +- **组不成队 / `ready=false`**：`MultiVehicleFleetNum` 与实际车数不符；从车 `MultiVehicleMasterEndpoint` 没指向主车端口；`soTag` 不匹配。 +- **摇杆无反应**：`MultiVehicleUseDetect=true` 但检测没锁（安全门）；或误把系数设成 <1 / 滑条乘零；或在从车而非主车上操作。 +- **两车一起匀速跑飞**：编队中心反推必须用**真正的 SE(2) 逆**（`SolveTransform2D`），不能用逐分量取负 `-layout`（从车 `layoutTh=180` 会算错 → 补偿正反馈发散）。已修复于 `InferFleetCenterFromCar`。 +- **检测框位置不对**：`TwoLegGuessX` 未随 `TestCarSyncDistance` 同步更新（见 §2 换算）。 +- **车号不对/全为 1**：`CarNum` 未通过 `[AsInitParam]`（`setp CarNum X`）按车配置。 + +--- + +## 7. 参考：Playground 双车验证值速查 + +```jsonc +// clumsy.json -> msConf （主车=agv_multi_1；从车把标注项改为车2值） +"TestCarSyncDistance": 2400, +"TestCarSyncTh": 0, +"DeltaDetectCenter": 800, +"TwoLegGuessX": -1600, +"MultiVehicleFleetNum": 2, +"MultiVehicleUseDetect": true, +"ManualCarSyncVxFac": 1.0, +"ManualCarSyncVthFac": 1.0, +"SyncThAccPerSec": 30, +"MultiVehicleMasterEndpoint": "/", // 从车: "127.0.0.1:8008" +"PlaygroundRobotName": "agv_multi_1", // 从车: "agv_multi_2" +"TwoLegLidarName": "rear_left_lidar_1,rear_right_lidar_1" // 从车: *_2 +``` + +```text +# Medulla startup.iocmd （主车） +loader SetShareObjectTag Multi1 # 从车: Multi2 +loader setp CarNum 1 # 从车: 2 +loader SetPlaygroundBinding agv_multi_1 tutorial-multi-wheel # 从车: agv_multi_2 +``` diff --git a/docs/MultiVehicleSync.md b/docs/MultiVehicleSync.md new file mode 100644 index 0000000..1b181fd --- /dev/null +++ b/docs/MultiVehicleSync.md @@ -0,0 +1,103 @@ +# 多车联动（MultiVehicleSync）算法原理 + +本文说明 **MDCSToolBox** 中 N 车联动的架构与几何原理。Tutorial `MultiWheelC` 与产品 `StandardMultiWheelLifterC` 均继承 `MultiWheelPilotDefinition`，调用 `InitMultiVehicleCoordination()` 启用同步。 + +## 1. 总体架构 + +每台 AGV 运行 **Detour + Medulla (M) + Clumsy (C)**： + +| 实例 | Detour `shareObjectTag` | Medulla/Clumsy `soTag` | Clumsy 角色 | `MultiVehicleMasterEndpoint` | Clumsy `port` | Detour HTTP | `CarNum` | +|------|-------------------------|------------------------|-------------|------------------------------|---------------|-------------|----------| +| AGV1 | `Multi1` | `Multi1` | 主车 master | `/` | 8008 | 4321 | 1 | +| AGV2 | `Multi2` | `Multi2` | 从车 slave | `127.0.0.1:8008` | 8009 | 4421 | 2 | + +Medulla web API（与 Clumsy 车队同步端口无关）：AGV1 **8007**，AGV2 **8108**（单机部署时 Medulla #2 不可占用 8008，该端口留给 Clumsy 主车）。 + +**同机隔离（Core 层）** + +- Detour 写入 `DObject("DetourPos{shareObjectTag}")`；Clumsy 读取 `DObject("DetourPos{soTag}")`，tag 必须一致。 +- Clumsy 通过 `detourHost`/`detourPort`（`clumsyconsole.json` 或 `clumsy.json`）访问对应 Detour HTTP 端口（4321/4421）。 +- 每台 Clumsy 绑定独立 `WebAPI.port`（8008/8009），register/notify 使用 `VehicleSyncInfo.Port` 寻址。 + +M 与 C 通过 `CartActivator` 共享 DObject（`MedullaCartUpperIO{tag}`），不直接 DLL 调用。 + +```text +从车 --GET /multi-vehicle-register--> 主车:8008 （上报本车位姿、layout、Port） +主车 --GET /multi-vehicle-notify----> 从车:8009 （广播车队中心、fleet 命令、完整 fleet 状态） +``` + +## 2. 数据模型 + +定义于 `MDCSToolBox/Clumsy/Pilot/MultiWheel/VehicleSyncModels.cs`。 + +### 2.1 VehicleSyncInfo（从车 → 主车） + +| 字段 | 含义 | +|------|------| +| `Master` | 是否主车 | +| `Ip` / `Port` | 本车同步端点（notify 寻址） | +| `PosAvailable` | 本车 Detour/SLAM 是否有效 | +| `X/Y/Th` | 本车世界位姿（mm, deg） | +| `LayoutX/Y/Th` | 本车在车队坐标系下的 **固定 slot** | +| `Aligned` | 本车互识别/精度是否满足（见 §4） | + +### 2.2 VehicleSyncNotification（主车 → 从车） + +除原 fleet 命令外，主车广播 `CenterX/Y/Th`（世界系车队中心）、`SyncTh/SyncDistance`、完整 `Fleet` 快照等。 + +## 3. 编队几何与车队中心 + +### 3.1 编队布局（双车） + +布局由 `TestCarSyncDistance` 与 `TestCarSyncTh` 唯一确定（不再有独立 slot 表）：每车相对车队中心沿编队方向对称分布于 ±`TestCarSyncDistance/2`，从车朝向额外翻转 180°。实现见 `GetLayoutPose`。`TestCarSyncDistance` 同时是检测环（`detectDistance = TestCarSyncDistance - DeltaDetectCenter`）与 SLAM 位姿环的唯一间距来源，二者据此自洽。 + +### 3.2 车队中心反推（非位姿平均） + +世界系中心 **不是** 对各车 X/Y/Th 取平均，而是由 **当前 `PosAvailable==true` 的车** 按固定 slot 反推： + +```text +Transform2D(fleetCenter, layoutSlot) = carWorldPose +⇒ fleetCenter = Transform2D(carWorld, -layoutSlot) // 实现见 InferFleetCenterFromCar +``` + +- 多台有效车：对各车反推的中心取平均（理论上应一致，差异可作互检）。 +- 零台有效车：无世界系中心 → **手动模式**仍可通过互识别保持编队；**自动模式**门控拒绝（见 §5）。 + +## 4. 互识别接口（插件实现） + +基类 `MultiWheelPilotDefinition` 提供虚方法： + +```csharp +protected virtual (bool Valid, float Dx, float Dy, float Dth) DetectNeighborBias(float detectDistance); +``` + +- 基类：`MultiVehicleUseDetect` 门控 → 计算 `Aligned` → `SendMotion(localCompensate*)`。 +- **具体检测**（雷达名、两腿/反光板等）由车体插件重写。 + - 产品：`StandardMultiWheelLifterC` → `VehicleMoverHelper.GetSyncBias` + - Tutorial：默认 `Valid=false`（无检测硬件时可纯靠手动+SLAM） + +## 5. 手动 / 自动门控 + +| 模式 | 条件 | +|------|------| +| 手动 | 始终可用；可无 Detour，靠 `DetectNeighborBias` 纠正 | +| 自动 | 主车要求 fleet 中 **至少 1 台** `PosAvailable`；否则 toast 拒绝并清零 `MultiVehicleAutoEnabled` | + +## 6. 主循环（默认 50ms） + +逻辑位于 `MultiWheelPilotDefinition.MultiVehicle.cs` 的 `TickMultiVehicle`： + +1. 判断 master/slave、手动/自动是否启用 +2. 读取本车 SLAM（若 `PosAvailable`） +3. 主车：计算 fleet 命令；从 notification 同步 +4. `GetLayoutPose` → 本车布局位姿（由 `TestCarSyncDistance`/`TestCarSyncTh` 推导） +5. `DetectNeighborBias`（可选）→ 互识别补偿 +6. 有有效中心时 SLAM `posBias` 补偿 +7. `SendMotion(fleetVx, FrontTh, RearTh, localCompensate*)` +8. master：register 汇总 + notify 各从车（按 `Ip:Port`，跳过自身 `CarNum`） + +## 7. 配置文件 + +见 [RunAndDeploy.md](./RunAndDeploy.md)：`clumsyconsole.json`（soTag/port/detour）、`clumsy.json`（msConf slot/主车端点）、`detour.json`（shareObjectTag/DetourPort）。 + +修复与迁移记录见 [BugFixes.md](./BugFixes.md)。 diff --git a/docs/RunAndDeploy.md b/docs/RunAndDeploy.md new file mode 100644 index 0000000..202f226 --- /dev/null +++ b/docs/RunAndDeploy.md @@ -0,0 +1,88 @@ +# 运行与部署说明 + +## 1. 依赖 + +所有 Tutorial 插件引用 `D:\MDCS\Release`： + +| 组件 | 路径 | +|------|------| +| Medulla | `Release\Medulla\` | +| Clumsy | `Release\Clumsy\ClumsyLite.exe` | +| CommonUsage | `Release\CommonUsage.dll` | +| MDCSToolBox | `Release\MDCSToolBox.dll`（由 `Products\mdcstoolbox` 编译 PostBuild 输出） | + +首次使用前编译 MDCSToolBox： + +```powershell +dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release +``` + +## 2. 编译 Tutorial + +```powershell +cd D:\MDCS\Source\Tutorial +dotnet build Tutorial.sln +``` + +输出： + +- M 插件 → `build\Medulla\plugins\` +- C 插件 → `build\Clumsy\` 与 `build\Clumsy_AGV2\`（PostBuild 复制运行时） + +## 3. 目录结构 + +```text +Tutorial/ + DiffWheel/DiffWheelM/ DiffWheel/DiffWheelC/ + MultiWheel/MultiWheelM/ MultiWheel/MultiWheelC/ + deploy/ # 配置模板（入库） + build/ # 运行目录（gitignore） +``` + +## 4. 单车验证（多舵轮） + +1. 启动 Playground（若使用仿真） +2. 启动 Detour（`DetourLite`，`detour.json` 中 `shareObjectTag=Multi1`，端口 4321） +3. `build\Medulla\start_medulla_agv1.bat` +4. 使用 `deploy\start_clumsy_agv1.bat` 或手动复制： + - `deploy\chassis.json` → `build\Clumsy\` + - `deploy\clumsy_agv1\clumsy.json` → `build\Clumsy\clumsy.json` + - `deploy\clumsy_agv1\clumsyconsole.json` → `build\Clumsy\clumsyconsole.json` +5. 在 `build\Clumsy\` 运行 `ClumsyLite.exe` + +## 5. 双车手动联动（同机） + +| 配置项 | AGV1（主车） | AGV2（从车） | +|--------|-------------|-------------| +| Medulla | `build\Medulla\` tag `Multi1`，web API **8007** | `build\Medulla_AGV2\` tag `Multi2`，web API **8108** | +| Detour | `detour.json` tag `Multi1`，HTTP **4321** / Web **4322** | `Detour_AGV2\detour.json` tag `Multi2`，HTTP **4421** / Web **4422** | +| `clumsyconsole.json` | `soTag=Multi1`, `port=8008`, `detourPort=4321` | `soTag=Multi2`, `port=8009`, `detourPort=4421` | +| `clumsy.json` msConf | `MultiVehicleMasterEndpoint="/"`, `CarNum=1` | `MultiVehicleMasterEndpoint="127.0.0.1:8008"`, `CarNum=2` | + +**tag 一致性**：Detour `shareObjectTag` = Clumsy `soTag` = Medulla `SetShareObjectTag`（见 `startup.iocmd`）。 + +1. 启动 Playground + 两台 Detour + 两台 Medulla + 两台 ClumsyLite（工作目录 `build\Clumsy` / `build\Clumsy_AGV2`），或使用 `DetourLite\bin\Debug\net8.0\start_all_sim.bat` 一键启动全部 7 个进程 +2. 主车 Medulla 遥控器按 **F** 开启「车队联动」 +3. 用摇杆驱动：Y=纵向，X=编队转向 + +## 6. 双车自动联动 + +1. 完成上一节双车启动 +2. 至少一台车 `PosAvailable=true`（Detour 定位有效） +3. 主车 Clumsy 触发自动联动或调度 `AGV.FleetGo(..., multiVehicleSync:true)` + +## 7. 差速示例 + +使用 `deploy\clumsy_diff\clumsy.json`，工作目录 `build\Clumsy_diff\`。 + +## 8. 配置文件说明 + +| 文件 | 作用 | +|------|------| +| `detour.json` | `shareObjectTag`、`guru.DetourPort`、雷达 layout | +| `clumsyconsole.json` | `soTag`、`port`、`allowMultiple`、`detourHost`/`detourPort` | +| `clumsy.json` | `msConf` → `PilotConfig`（含 `TestCarSyncDistance`、主车端点） | +| `chassis.json` | 四轮舵轮几何 | +| `startup.iocmd` | `SetShareObjectTag Multi1/Multi2` | + +详细算法见 [MultiVehicleSync.md](./MultiVehicleSync.md)，修复清单见 [BugFixes.md](./BugFixes.md)。