From 1e780c2b65b3b4825b81ba4fcc9068c9046e235e Mon Sep 17 00:00:00 2001 From: ykkokluo <984278063@qq.com> Date: Fri, 14 Aug 2026 09:49:23 +0800 Subject: [PATCH] =?UTF-8?q?2.0=E5=8D=8F=E8=AE=AE=E5=AE=8C=E5=96=84?= =?UTF-8?q?=E5=8A=A0=E7=AE=80=E5=8D=95=E4=BA=A4=E7=AE=A1=E9=85=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Chained/AbstractLoopMission.cs | 7 +- StandardScene.Core/Commons.cs | 6 +- StandardScene.Fass2Simulator/App.xaml.cs | 2 + .../Fass2SimActionEngine.cs | 51 +- .../Fass2SimActionResolver.cs | 63 +- .../Fass2SimBootstrap.cs | 22 +- .../Fass2SimMotionEngine.cs | 59 +- .../Fass2SimProtocol.cs | 11 + .../Fass2SimSelfTests.cs | 6 +- .../Fass2SimUdpHost.cs | 2 +- .../StandardScene.Fass2Simulator.csproj | 1 + .../appsettings.car2.json | 25 + StandardScene.Fass2Simulator/appsettings.json | 4 +- .../Protocol/Fass2ProtocolGoldenTests.cs | 171 ++++ .../Fass2SimMagneticProtocolCrossTests.cs | 63 ++ .../Protocol/Fass2UdpHubMultiCarTests.cs | 95 +++ .../StandardScene.Magnetic.Tests.csproj | 27 + .../Tasking/Fass2ControlAreaGateTests.cs | 164 ++++ .../Fass2ControlStopStateMachineTests.cs | 102 +++ .../Fass2ReconnectChainSelectorTests.cs | 83 ++ .../Tasking/Fass2SimControlStopTests.cs | 62 ++ .../TestHelpers/Fass2TestFrameBuilder.cs | 69 ++ .../TestHelpers/TestUdpCar.cs | 64 ++ .../CarTypes/{MagFass2Car.cs => Mag2Car.cs} | 674 ++++++++++++--- .../Chained/Mag2LoopMission.cs | 209 +++++ .../Chained/MagFass2LoopMission.cs | 20 - .../MagneticSceneProfile.cs | 10 +- .../Properties/InternalsVisibleTo.cs | 3 + .../Protocol/Fass2StateCodec.cs | 2 +- .../Protocol/Fass2UdpHub.cs | 2 +- .../StandardScene.Magnetic.scene.json | 4 +- .../Tasking/Fass2ActionResolver.cs | 94 +- .../Tasking/Fass2ControlAreaGate.cs | 214 +++++ .../Tasking/Fass2ControlAreaOccupancy.cs | 79 ++ .../Tasking/Fass2PathFinder.cs | 2 +- .../Tasking/Fass2ReconnectChainSelector.cs | 127 +++ .../Tasking/Fass2RouteHelper.cs | 210 +++++ .../Tasking/Fass2SiteFieldReader.cs | 44 + .../Tasking/Fass2SiteFields.cs | 6 + .../Tasking/Fass2TaskBuilder.cs | 79 +- .../Tasking/Fass2TaskContext.cs | 44 +- .../Tasking/Fass2TaskPlan.cs | 2 +- .../Tasking/Fass2TaskStateMachine.cs | 806 ++++++++++++++++-- .../Tasking/Fass2TrafficLocker.cs | 303 +++++++ ...2CarFileLogger.cs => Mag2CarFileLogger.cs} | 6 +- ...2Car使用指南.html => Mag2Car使用指南.html} | 68 +- ...Fass2Car使用指南.md => Mag2Car使用指南.md} | 104 ++- ...ss2Car使用指南.pdf => Mag2Car使用指南.pdf} | Bin StandardScene.Magnetic/docs/export-pdf.ps1 | 8 +- StandardScene.sln | 66 ++ scripts/run-fass2-multi-car-udp.ps1 | 61 ++ 51 files changed, 4013 insertions(+), 393 deletions(-) create mode 100644 StandardScene.Fass2Simulator/appsettings.car2.json create mode 100644 StandardScene.Magnetic.Tests/Protocol/Fass2ProtocolGoldenTests.cs create mode 100644 StandardScene.Magnetic.Tests/Protocol/Fass2SimMagneticProtocolCrossTests.cs create mode 100644 StandardScene.Magnetic.Tests/Protocol/Fass2UdpHubMultiCarTests.cs create mode 100644 StandardScene.Magnetic.Tests/StandardScene.Magnetic.Tests.csproj create mode 100644 StandardScene.Magnetic.Tests/Tasking/Fass2ControlAreaGateTests.cs create mode 100644 StandardScene.Magnetic.Tests/Tasking/Fass2ControlStopStateMachineTests.cs create mode 100644 StandardScene.Magnetic.Tests/Tasking/Fass2ReconnectChainSelectorTests.cs create mode 100644 StandardScene.Magnetic.Tests/Tasking/Fass2SimControlStopTests.cs create mode 100644 StandardScene.Magnetic.Tests/TestHelpers/Fass2TestFrameBuilder.cs create mode 100644 StandardScene.Magnetic.Tests/TestHelpers/TestUdpCar.cs rename StandardScene.Magnetic/CarTypes/{MagFass2Car.cs => Mag2Car.cs} (68%) create mode 100644 StandardScene.Magnetic/Chained/Mag2LoopMission.cs delete mode 100644 StandardScene.Magnetic/Chained/MagFass2LoopMission.cs create mode 100644 StandardScene.Magnetic/Properties/InternalsVisibleTo.cs create mode 100644 StandardScene.Magnetic/Tasking/Fass2ControlAreaGate.cs create mode 100644 StandardScene.Magnetic/Tasking/Fass2ControlAreaOccupancy.cs create mode 100644 StandardScene.Magnetic/Tasking/Fass2ReconnectChainSelector.cs create mode 100644 StandardScene.Magnetic/Tasking/Fass2RouteHelper.cs create mode 100644 StandardScene.Magnetic/Tasking/Fass2TrafficLocker.cs rename StandardScene.Magnetic/Tasking/{MagFass2CarFileLogger.cs => Mag2CarFileLogger.cs} (89%) rename StandardScene.Magnetic/docs/{MagFass2Car使用指南.html => Mag2Car使用指南.html} (95%) rename StandardScene.Magnetic/docs/{MagFass2Car使用指南.md => Mag2Car使用指南.md} (86%) rename StandardScene.Magnetic/docs/{MagFass2Car使用指南.pdf => Mag2Car使用指南.pdf} (100%) create mode 100644 scripts/run-fass2-multi-car-udp.ps1 diff --git a/StandardScene.Core/Chained/AbstractLoopMission.cs b/StandardScene.Core/Chained/AbstractLoopMission.cs index 983fc0e..190323a 100644 --- a/StandardScene.Core/Chained/AbstractLoopMission.cs +++ b/StandardScene.Core/Chained/AbstractLoopMission.cs @@ -335,7 +335,7 @@ namespace StandardScene.Chained /// /// 站点ID /// 找到的车辆，未找到返回 null - protected Car FindCarArrivedAtSite(int siteId) + protected virtual Car FindCarArrivedAtSite(int siteId) { try { @@ -1027,7 +1027,6 @@ namespace StandardScene.Chained /// 是否倒车，默认为 false public async Task GoSite(AbstractCar car, Site targetSite, string action = "/", bool reverse = false) { - try { // 创建路径规划 @@ -1038,8 +1037,8 @@ namespace StandardScene.Chained // 查找从当前位置到目标站点的路径 plan.FindRoute(SimpleLib.GetSite(car.GetLastSite()), targetSite); - - // 编译并执行移动脚本 + + // 编译并执行移动脚本（默认 Forecast） var program = plan.Compile("move"); // 标记车辆为占用状态 diff --git a/StandardScene.Core/Commons.cs b/StandardScene.Core/Commons.cs index 33b51fe..b0cb2ea 100644 --- a/StandardScene.Core/Commons.cs +++ b/StandardScene.Core/Commons.cs @@ -297,11 +297,13 @@ namespace StandardScene && car.status.pendingLocks.Length == 0 && !car.tags.Contains("deliver") &&//pendingLocks临时增加解决车接多任务问题 (!car.tags.Contains("shouldCharge") || needCharge) && !car.tags.Contains("currentWorkStep") && !car.fields.ContainsKey("StopAccept") && car.tags.Contains("Online"); //巡航任务未完成的车 - if (car.name.Contains("模拟")) + // Mag2Car 联调：UDP Online 即可调度，不强制 lstatus=上线（模拟器常报 state=0 未准备） + var isMag2Car = string.Equals(car.GetType().Name, "Mag2Car", StringComparison.Ordinal); + if (car.name.Contains("模拟") || isMag2Car) { conditions = car.GetLastSite() != -1 && (!car.tags.Contains("holdCar") || !checkHoldCar) && !car.tags.Contains("occupied") - + && (!isMag2Car || car.tags.Contains("Online")) && car.status.pendingLocks.Length == 0 && !car.tags.Contains("deliver") &&//pendingLocks临时增加解决车接多任务问题 (!car.tags.Contains("shouldCharge") || needCharge) && !car.tags.Contains("currentWorkStep") && !car.fields.ContainsKey("StopAccept"); //巡航任务未完成的车 diff --git a/StandardScene.Fass2Simulator/App.xaml.cs b/StandardScene.Fass2Simulator/App.xaml.cs index 895d57f..2ab235a 100644 --- a/StandardScene.Fass2Simulator/App.xaml.cs +++ b/StandardScene.Fass2Simulator/App.xaml.cs @@ -7,6 +7,8 @@ namespace StandardScene.Fass2Simulator { protected override void OnStartup(StartupEventArgs e) { + Fass2SimBootstrap.ApplyCommandLine(e.Args); + if (HasTestArg(e.Args)) { var code = Fass2SimSelfTests.Run(e.Args); diff --git a/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs b/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs index 581bbb2..7d0d5fd 100644 --- a/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs +++ b/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs @@ -58,6 +58,15 @@ namespace StandardScene.Fass2Simulator } } + public void ResetForNewTask() + { + lock (_syncRoot) + { + _pendingActions.Clear(); + _expectedStation = null; + } + } + public void OnActionCommand(ulong actionId, Fass2SimNodeMessage patch) { if (patch == null) @@ -67,13 +76,16 @@ namespace StandardScene.Fass2Simulator lock (_syncRoot) { + var targetNode = patch.Node != 0 ? patch.Node : _vehicle.Node; if (patch.Node != 0 && patch.Node != _vehicle.Node) { Fass2SimLog.WriteLine( - $"[{Now()}] -> 0xA1 节点不匹配: patch={patch.Node}, current={_vehicle.Node}，仍尝试应用"); + $"[{Now()}] -> 0xA1 忽略: patch node={patch.Node}, current={_vehicle.Node}"); + return; } - var delay = ResolveActionDelayMs(patch.Node != 0 ? patch.Node : _vehicle.Node); + var delay = ResolveActionDelayMs(targetNode); + MergeExpectedLocked(patch); SchedulePatch(patch, actionId, 0, delay); Fass2SimLog.WriteLine( $"[{Now()}] -> 0xA1 actionId={actionId} 已排队，delay={delay}ms, pending={DescribePatch(patch)}"); @@ -161,6 +173,37 @@ namespace StandardScene.Fass2Simulator } } + public void MergeExpected(Fass2SimNodeMessage patch) + { + if (patch == null) + { + return; + } + + lock (_syncRoot) + { + MergeExpectedLocked(patch); + } + } + + private void MergeExpectedLocked(Fass2SimNodeMessage patch) + { + if (_expectedStation == null) + { + return; + } + + if (patch.Node != 0 && patch.Node != _expectedStation.Node) + { + return; + } + + if (patch.StartStop > 0) + { + _expectedStation.StartStop = patch.StartStop; + } + } + private bool IsStationCompleteLocked() { if (_pendingActions.Count > 0) @@ -229,9 +272,9 @@ namespace StandardScene.Fass2Simulator private void ApplyPatch(Fass2SimNodeMessage patch) { - if (patch.Node != 0) + if (patch.Node != 0 && patch.Node != _vehicle.Node) { - _vehicle.Node = patch.Node; + return; } if (patch.StartStop > 0) diff --git a/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs b/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs index 6658a2a..a989e05 100644 --- a/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs +++ b/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs @@ -10,13 +10,14 @@ namespace StandardScene.Fass2Simulator { public const byte StartStopPass = 1; public const byte StartStopStop = 2; + public const byte StartStopControlStart = 11; + public const byte StartStopControlStop = 12; public const byte StartStopPrecision = 22; - private static readonly (string Name, Func Get)[] ActionFields = + public static bool VerifyTrajectoryFields { get; set; } + + private static readonly (string Name, Func Get)[] MechanismFields = { - ("Direction", n => n.Direction), - ("Orientation", n => n.Orientation), - ("Byroad", n => n.Byroad), ("Obstacle", n => n.Obstacle), ("Audio", n => n.Audio), ("Light", n => n.Light), @@ -29,6 +30,18 @@ namespace StandardScene.Fass2Simulator ("Shutdown", n => n.Shutdown) }; + private static readonly (string Name, Func Get)[] TrajectoryFields = + { + ("Direction", n => n.Direction), + ("Orientation", n => n.Orientation), + ("Byroad", n => n.Byroad) + }; + + public static bool AllowsAutoContinue(byte startStop) + { + return startStop == StartStopPass || startStop == StartStopControlStart; + } + public static bool RequiresActionWait(Fass2SimNodeMessage expected) { if (expected == null) @@ -36,12 +49,25 @@ namespace StandardScene.Fass2Simulator return false; } - if (expected.StartStop is StartStopStop or StartStopPrecision) + if (expected.StartStop is StartStopStop or StartStopPrecision or StartStopControlStop or StartStopControlStart) { return true; } - foreach (var field in ActionFields) + foreach (var field in MechanismFields) + { + if (field.Get(expected) != 0) + { + return true; + } + } + + if (!VerifyTrajectoryFields) + { + return false; + } + + foreach (var field in TrajectoryFields) { if (field.Get(expected) != 0) { @@ -64,7 +90,7 @@ namespace StandardScene.Fass2Simulator return false; } - if (vehicleState == 1) + if (vehicleState is 1 or 5) { return false; } @@ -79,6 +105,11 @@ namespace StandardScene.Fass2Simulator return true; } + if (expected.StartStop == StartStopControlStop) + { + return false; + } + return ListPendingFields(expected, actual).Count == 0; } @@ -95,12 +126,24 @@ namespace StandardScene.Fass2Simulator pending.Add("StartStop"); } - if (expected.Speed != 0 && actual.Speed != expected.Speed) + if (VerifyTrajectoryFields) { - pending.Add("Speed"); + if (expected.Speed != 0 && actual.Speed != expected.Speed) + { + pending.Add("Speed"); + } + + foreach (var field in TrajectoryFields) + { + var expectedValue = field.Get(expected); + if (expectedValue != 0 && GetActualField(actual, field.Name) != expectedValue) + { + pending.Add(field.Name); + } + } } - foreach (var field in ActionFields) + foreach (var field in MechanismFields) { var expectedValue = field.Get(expected); if (expectedValue != 0 && GetActualField(actual, field.Name) != expectedValue) diff --git a/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs b/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs index 2d7fe84..91dce4d 100644 --- a/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs +++ b/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs @@ -8,12 +8,30 @@ namespace StandardScene.Fass2Simulator { public static class Fass2SimBootstrap { - public static Fass2SimConfig LoadConfig() + public static string SettingsFile { get; private set; } = "appsettings.json"; + + public static void ApplyCommandLine(string[] args) + { + if (args == null) + { + return; + } + + for (var i = 0; i < args.Length - 1; i++) + { + if (string.Equals(args[i], "--config", StringComparison.OrdinalIgnoreCase)) + { + SettingsFile = args[i + 1]; + } + } + } + + public static Fass2SimConfig LoadConfig(string settingsFile = null) { var basePath = AppContext.BaseDirectory; var builder = new ConfigurationBuilder() .SetBasePath(basePath) - .AddJsonFile("appsettings.json", optional: true, reloadOnChange: false); + .AddJsonFile(settingsFile ?? SettingsFile, optional: true, reloadOnChange: false); var configuration = builder.Build(); var config = new Fass2SimConfig(); diff --git a/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs b/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs index caa4d1d..b1d5273 100644 --- a/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs +++ b/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs @@ -41,6 +41,12 @@ namespace StandardScene.Fass2Simulator _windowNodes = nodes; _vehicle.Task = taskId; + if (taskChanged) + { + _actions.ResetForNewTask(); + ResetSegment(); + } + ApplyCurrentNodeFields(nodes); LogNodes(nodes); @@ -63,10 +69,35 @@ namespace StandardScene.Fass2Simulator } } + if (!ContainsNode(nodes, _vehicle.Node)) + { + Fass2SimLog.WriteLine( + $"[{Now()}] -> 0xB1 窗口不含当前 node={_vehicle.Node}，忽略启动路段（避免回退到窗口首站）"); + return; + } + TryStartNextSegment(); } } + private static bool ContainsNode(Fass2SimNodeMessage[] nodes, ushort node) + { + if (nodes == null) + { + return false; + } + + for (var i = 0; i < nodes.Length; i++) + { + if (nodes[i].Node == node) + { + return true; + } + } + + return false; + } + public void OnStartCommand() { lock (_syncRoot) @@ -147,7 +178,7 @@ namespace StandardScene.Fass2Simulator } _segmentTarget = target; - _segmentLength = ResolveSegmentLength(target); + _segmentLength = ResolveSegmentLength(_vehicle.Node, target); _segmentProgress = 0; _segmentActive = true; _vehicle.State = 1; @@ -184,7 +215,7 @@ namespace StandardScene.Fass2Simulator } var expected = _actions.ExpectedStation; - if (expected == null || expected.StartStop != Fass2SimActionResolver.StartStopPass) + if (expected == null || !Fass2SimActionResolver.AllowsAutoContinue(expected.StartStop)) { return; } @@ -218,15 +249,24 @@ namespace StandardScene.Fass2Simulator return null; } + // 必须先在窗口内定位当前站，再取其后一站。 + // 旧逻辑在窗口不含当前站时会落到 nodes[0]，把更早的站当成下一目标 → 车体 Node 瞬间回退/瞬移。 + var foundCurrent = false; var startIndex = 0; for (var i = 0; i < nodes.Length; i++) { if (nodes[i].Node == currentNode) { + foundCurrent = true; startIndex = i + 1; } } + if (!foundCurrent) + { + return null; + } + for (var i = startIndex; i < nodes.Length; i++) { var node = nodes[i]; @@ -248,12 +288,21 @@ namespace StandardScene.Fass2Simulator if (node.Node == _vehicle.Node && node.StartStop > 0) { _vehicle.StartStop = node.StartStop; + _actions.MergeExpected(node); } } } - private double ResolveSegmentLength(Fass2SimNodeMessage target) + private double ResolveSegmentLength(ushort fromNode, Fass2SimNodeMessage target) { + foreach (var node in _windowNodes) + { + if (node.Node == fromNode && node.Distance > 0) + { + return node.Distance; + } + } + if (target.Distance > 0) { return target.Distance; @@ -272,12 +321,12 @@ namespace StandardScene.Fass2Simulator { if (target.Speed > 0) { - return target.Speed; + return Fass2SimProtocol.ProtocolSpeedToMmPerSec(target.Speed); } if (_vehicle.Speed > 0) { - return _vehicle.Speed; + return Fass2SimProtocol.ProtocolSpeedToMmPerSec(_vehicle.Speed); } return _config.DefaultSpeed; diff --git a/StandardScene.Fass2Simulator/Fass2SimProtocol.cs b/StandardScene.Fass2Simulator/Fass2SimProtocol.cs index b6c1d3e..ace386c 100644 --- a/StandardScene.Fass2Simulator/Fass2SimProtocol.cs +++ b/StandardScene.Fass2Simulator/Fass2SimProtocol.cs @@ -152,6 +152,17 @@ namespace StandardScene.Fass2Simulator } } + /// 协议速度(0.1 m/min) → mm/s。 + public static double ProtocolSpeedToMmPerSec(ushort protocolSpeed) + { + if (protocolSpeed == 0) + { + return 0; + } + + return protocolSpeed * 0.1 * 1000.0 / 60.0; + } + public static string ToHex(byte[] bytes) { return bytes == null ? string.Empty : string.Join(" ", bytes.Select(b => b.ToString("X2"))); diff --git a/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs b/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs index dacef71..09c9416 100644 --- a/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs +++ b/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs @@ -21,6 +21,7 @@ namespace StandardScene.Fass2Simulator private static int RunMotionSelfTest() { + Fass2SimFileLogger.Configure(new Fass2SimConfig { EnableFileLog = false }); var config = new Fass2SimConfig { InitialNode = 1, @@ -31,9 +32,9 @@ namespace StandardScene.Fass2Simulator var (vehicle, motion, _) = Fass2SimBootstrap.CreateTestStack(config); var nodes = new[] { - new Fass2SimNodeMessage { Node = 1, StartStop = 1 }, + new Fass2SimNodeMessage { Node = 1, StartStop = 1, Distance = 500 }, new Fass2SimNodeMessage { Node = 2, StartStop = 1, Distance = 500 }, - new Fass2SimNodeMessage { Node = 3, StartStop = 2, Distance = 500 } + new Fass2SimNodeMessage { Node = 3, StartStop = 2 } }; vehicle.State = 1; @@ -57,6 +58,7 @@ namespace StandardScene.Fass2Simulator private static int RunActionSelfTest() { + Fass2SimFileLogger.Configure(new Fass2SimConfig { EnableFileLog = false }); var config = new Fass2SimConfig { InitialNode = 3, diff --git a/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs b/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs index 124cd68..87c0646 100644 --- a/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs +++ b/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs @@ -69,7 +69,7 @@ namespace StandardScene.Fass2Simulator Fass2SimLog.WriteLine($" 路段模拟 : dist={_config.DefaultSegmentDistance}mm 或 node.Distance, speed={_config.DefaultSpeed}mm/s"); Fass2SimLog.WriteLine($" 动作延时 : {_config.ActionDelayMs}ms（0xA1 到位），AutoComplete={_config.AutoCompleteStationActions}"); Fass2SimLog.WriteLine($" 初始节点 : {_config.InitialNode}, 状态={Fass2SimProtocol.StateText(_config.InitialState)}"); - Fass2SimLog.WriteLine("等待 MagFass2Car 联调（address=127.0.0.1, Port=5000, ListenPort=20103, VehicleCode=1）"); + Fass2SimLog.WriteLine("等待 Mag2Car 联调（address=127.0.0.1, Port=5000, ListenPort=20103, VehicleCode=1）"); } public void Stop() diff --git a/StandardScene.Fass2Simulator/StandardScene.Fass2Simulator.csproj b/StandardScene.Fass2Simulator/StandardScene.Fass2Simulator.csproj index cb825ef..4aca1f6 100644 --- a/StandardScene.Fass2Simulator/StandardScene.Fass2Simulator.csproj +++ b/StandardScene.Fass2Simulator/StandardScene.Fass2Simulator.csproj @@ -21,6 +21,7 @@ + diff --git a/StandardScene.Fass2Simulator/appsettings.car2.json b/StandardScene.Fass2Simulator/appsettings.car2.json new file mode 100644 index 0000000..2fae109 --- /dev/null +++ b/StandardScene.Fass2Simulator/appsettings.car2.json @@ -0,0 +1,25 @@ +{ + "Fass2Simulator": { + "VehicleCode": 2, + "VehicleListenAddress": "0.0.0.0", + "VehicleListenPort": 5002, + "SchedulerHost": "127.0.0.1", + "SchedulerListenPort": 20103, + "ReportIntervalMs": 200, + "InitialNode": 5, + "InitialState": 1, + "BatteryCharge": 100, + "CarLength": 1200, + "CarWidth": 800, + "LogRawFrames": true, + "DefaultSpeed": 500, + "DefaultSegmentDistance": 1000, + "SecondsPerSegment": 2, + "AutoContinueOnPass": true, + "ActionDelayMs": 1000, + "AutoCompleteStationActions": false, + "NodeProfilesPath": "sim-nodes.json", + "EnableFileLog": true, + "LogDirectory": "logs\\car2" + } +} diff --git a/StandardScene.Fass2Simulator/appsettings.json b/StandardScene.Fass2Simulator/appsettings.json index e97fd60..1b0ddd4 100644 --- a/StandardScene.Fass2Simulator/appsettings.json +++ b/StandardScene.Fass2Simulator/appsettings.json @@ -2,12 +2,12 @@ "Fass2Simulator": { "VehicleCode": 1, "VehicleListenAddress": "0.0.0.0", - "VehicleListenPort": 5000, + "VehicleListenPort": 5001, "SchedulerHost": "127.0.0.1", "SchedulerListenPort": 20103, "ReportIntervalMs": 200, "InitialNode": 1, - "InitialState": 0, + "InitialState": 1, "BatteryCharge": 100, "CarLength": 1200, "CarWidth": 800, diff --git a/StandardScene.Magnetic.Tests/Protocol/Fass2ProtocolGoldenTests.cs b/StandardScene.Magnetic.Tests/Protocol/Fass2ProtocolGoldenTests.cs new file mode 100644 index 0000000..941fe3f --- /dev/null +++ b/StandardScene.Magnetic.Tests/Protocol/Fass2ProtocolGoldenTests.cs @@ -0,0 +1,171 @@ +using StandardScene.Magnetic.Protocol; +using StandardScene.Magnetic.Tasking; +using StandardScene.Magnetic.Tests.TestHelpers; +using System; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Protocol +{ + public sealed class Fass2ProtocolGoldenTests + { + [Fact] + public void BuildControl_Start_Car1_Param0_MatchesGoldenLayout() + { + var frame = Fass2Protocol.BuildControl(Fass2Protocol.CmdStart, 1, 0); + + Assert.Equal(Fass2Protocol.ControlFrameLength, frame.Length); + Assert.Equal(Fass2Protocol.Begin, frame[0]); + Assert.Equal(Fass2Protocol.CmdStart, frame[1]); + Assert.Equal(0x01, frame[2]); + Assert.Equal(0x00, frame[3]); + Assert.Equal(0x00, frame[4]); + Assert.Equal(0x00, frame[5]); + Assert.Equal(Fass2Protocol.Xor(frame, 1, 47), frame[48]); + Assert.Equal(Fass2Protocol.End, frame[49]); + + const string goldenPrefix = "BB 01 01 00 00 00"; + Assert.StartsWith(goldenPrefix, Fass2TestFrameBuilder.ToHex(frame)); + } + + [Fact] + public void BuildNodes_TwoNodes_LittleEndianAndXor() + { + var nodes = new[] + { + new Fass2NodeMessage + { + Node = 1, + Distance = 500, + StartStop = 1, + Speed = 120 + }, + new Fass2NodeMessage + { + Node = 2, + StartStop = 2, + Lift = 1 + } + }; + + var frame = Fass2Protocol.BuildNodes(1, 0x0102030405060708UL, nodes); + + Assert.Equal(Fass2Protocol.NodesFrameLength, frame.Length); + Assert.Equal(Fass2Protocol.CmdNodes, frame[1]); + Assert.Equal(0x08, frame[4]); + Assert.Equal(0x01, frame[11]); + Assert.Equal(0x02, frame[12]); + Assert.Equal(0x00, frame[13]); + Assert.Equal(Fass2Protocol.Xor(frame, 1, 297), frame[298]); + Assert.Equal(Fass2Protocol.End, frame[299]); + + var parsedNode0 = Fass2NodeMessage.FromBytes(CopyRange(frame, 14, 25)); + Assert.Equal((ushort)1, parsedNode0.Node); + Assert.Equal((ushort)500, parsedNode0.Distance); + Assert.Equal((byte)1, parsedNode0.StartStop); + Assert.Equal((ushort)120, parsedNode0.Speed); + + var parsedNode1 = Fass2NodeMessage.FromBytes(CopyRange(frame, 39, 25)); + Assert.Equal((ushort)2, parsedNode1.Node); + Assert.Equal((byte)2, parsedNode1.StartStop); + Assert.Equal((byte)1, parsedNode1.Lift); + } + + [Fact] + public void BuildAction_RoundTripsNodeBlock() + { + var node = new Fass2NodeMessage + { + Node = 3, + StartStop = 22, + Lift = 2, + Speed = 120 + }; + + var frame = Fass2Protocol.BuildAction(2, 999, node); + + Assert.Equal(Fass2Protocol.ActionFrameLength, frame.Length); + Assert.Equal(Fass2Protocol.CmdAction, frame[1]); + Assert.Equal(0x02, frame[2]); + Assert.Equal(0x00, frame[3]); + Assert.Equal(Fass2Protocol.Xor(frame, 1, 97), frame[98]); + + var parsed = Fass2NodeMessage.FromBytes(CopyRange(frame, 14, 25)); + Assert.Equal(node.Node, parsed.Node); + Assert.Equal(node.StartStop, parsed.StartStop); + Assert.Equal(node.Lift, parsed.Lift); + Assert.Equal(node.Speed, parsed.Speed); + } + + [Fact] + public void BuildStateResponse_FixedTimestamp_EncodesBeijingCalendar() + { + var frame = Fass2Protocol.BuildStateResponse(1, 1_704_067_200_000UL); + + Assert.Equal(Fass2Protocol.CmdStateResponse, frame[1]); + Assert.Equal(0x01, frame[2]); + Assert.Equal(0x00, frame[3]); + Assert.Equal(Fass2Protocol.Xor(frame, 1, 47), frame[48]); + Assert.Equal(Fass2Protocol.End, frame[49]); + } + + [Fact] + public void ParseState_RoundTripsMinimalFrame() + { + var frame = Fass2TestFrameBuilder.BuildStateFrame(5, 2, 7, 42); + + Assert.True(Fass2Protocol.TryExtractStateFrame(frame, frame.Length, out var extracted)); + Assert.Equal(frame, extracted); + + var report = Fass2Protocol.ParseState(frame); + Assert.Equal((ushort)5, report.Car); + Assert.Equal((byte)2, report.State); + Assert.Equal((ulong)42, report.Task); + Assert.Equal((ushort)7, report.Node.Node); + Assert.Equal((ushort)500, report.Node.Distance); + } + + [Fact] + public void NodeMessage_ToBytes_FromBytes_RoundTrip() + { + var source = new Fass2NodeMessage + { + Node = 11, + Distance = 1234, + StartStop = 2, + Direction = 3, + Orientation = 64, + Byroad = 2, + Speed = 120, + Lift = 1, + Roll = 2, + Shutdown = 1 + }; + + var roundTrip = Fass2NodeMessage.FromBytes(source.ToBytes()); + Assert.Equal(source.Node, roundTrip.Node); + Assert.Equal(source.Distance, roundTrip.Distance); + Assert.Equal(source.StartStop, roundTrip.StartStop); + Assert.Equal(source.Direction, roundTrip.Direction); + Assert.Equal(source.Orientation, roundTrip.Orientation); + Assert.Equal(source.Byroad, roundTrip.Byroad); + Assert.Equal(source.Speed, roundTrip.Speed); + Assert.Equal(source.Lift, roundTrip.Lift); + Assert.Equal(source.Roll, roundTrip.Roll); + Assert.Equal(source.Shutdown, roundTrip.Shutdown); + } + + [Fact] + public void SpeedToProtocol_UsesMPerMinTimesTen() + { + Assert.Equal((ushort)120, Fass2TaskBuilder.SpeedToProtocol(12)); + Assert.Equal((ushort)120, Fass2TaskBuilder.SpeedToProtocol(0.2)); + } + + private static byte[] CopyRange(byte[] source, int offset, int length) + { + var copy = new byte[length]; + Buffer.BlockCopy(source, offset, copy, 0, length); + return copy; + } + } +} diff --git a/StandardScene.Magnetic.Tests/Protocol/Fass2SimMagneticProtocolCrossTests.cs b/StandardScene.Magnetic.Tests/Protocol/Fass2SimMagneticProtocolCrossTests.cs new file mode 100644 index 0000000..e0cbfba --- /dev/null +++ b/StandardScene.Magnetic.Tests/Protocol/Fass2SimMagneticProtocolCrossTests.cs @@ -0,0 +1,63 @@ +using StandardScene.Fass2Simulator; +using StandardScene.Magnetic.Protocol; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Protocol +{ + /// + /// 验证 Magnetic 编解码与模拟器 100B 状态帧字段布局一致。 + /// + public sealed class Fass2SimMagneticProtocolCrossTests + { + [Fact] + public void StateFrame_NodeBlockOffset_MatchesMagneticParser() + { + var vehicle = new Fass2SimVehicle(new Fass2SimConfig + { + VehicleCode = 1, + InitialNode = 9, + InitialState = 2 + }); + vehicle.StartStop = 2; + vehicle.Distance = 777; + vehicle.Speed = 120; + vehicle.Lift = 1; + vehicle.Task = 1001; + + var simFrame = Fass2SimProtocol.BuildState(vehicle.Snapshot()); + var report = Fass2Protocol.ParseState(simFrame); + + Assert.Equal((ushort)1, report.Car); + Assert.Equal((byte)2, report.State); + Assert.Equal((ulong)1001, report.Task); + Assert.Equal((ushort)9, report.Node.Node); + Assert.Equal((ushort)777, report.Node.Distance); + Assert.Equal((byte)2, report.Node.StartStop); + Assert.Equal((ushort)120, report.Node.Speed); + Assert.Equal((byte)1, report.Node.Lift); + } + + [Fact] + public void NodeBytes_Are25BytesAligned() + { + var node = new Fass2NodeMessage + { + Node = 4, + Distance = 100, + StartStop = 1, + Byroad = 2, + Speed = 60 + }; + + var bytes = node.ToBytes(); + Assert.Equal(25, bytes.Length); + + var simNode = Fass2SimNodeMessage.FromBytes(bytes, 0); + Assert.Equal((ushort)4, simNode.Node); + Assert.Equal((ushort)100, simNode.Distance); + Assert.Equal((byte)1, simNode.StartStop); + Assert.Equal((byte)2, simNode.Byroad); + Assert.Equal((ushort)60, simNode.Speed); + } + } +} diff --git a/StandardScene.Magnetic.Tests/Protocol/Fass2UdpHubMultiCarTests.cs b/StandardScene.Magnetic.Tests/Protocol/Fass2UdpHubMultiCarTests.cs new file mode 100644 index 0000000..662752f --- /dev/null +++ b/StandardScene.Magnetic.Tests/Protocol/Fass2UdpHubMultiCarTests.cs @@ -0,0 +1,95 @@ +using StandardScene.Magnetic.Protocol; +using StandardScene.Magnetic.Tests.TestHelpers; +using System; +using System.Net; +using System.Net.Sockets; +using System.Threading; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Protocol +{ + public sealed class Fass2UdpHubMultiCarTests : IDisposable + { + private readonly int _hubPort; + private readonly TestUdpCar _car1; + private readonly TestUdpCar _car2; + + public Fass2UdpHubMultiCarTests() + { + _hubPort = AllocateUdpPort(); + _car1 = new TestUdpCar(1, AllocateUdpPort()); + _car2 = new TestUdpCar(2, AllocateUdpPort()); + + Fass2UdpHub.Register(_car1); + Fass2UdpHub.Register(_car2); + Fass2UdpHub.EnsureStarted(_hubPort); + Thread.Sleep(100); + } + + [Fact] + public void Hub_RoutesStateToRegisteredCar_AndRepliesWith0x10() + { + SendState(_car1.VehicleCode, 2, 10, 100); + var ack1 = _car1.WaitForAck(); + Assert.NotNull(ack1); + Assert.Equal(Fass2Protocol.ControlFrameLength, ack1.Length); + Assert.Equal(Fass2Protocol.CmdStateResponse, ack1[1]); + Assert.Equal(0x01, ack1[2]); + Assert.Single(_car1.Reports); + Assert.Equal((ushort)10, _car1.Reports[0].Node.Node); + Assert.Empty(_car2.Reports); + } + + [Fact] + public void Hub_DispatchesMultipleCarsIndependently() + { + SendState(1, 1, 11, 201); + SendState(2, 2, 22, 202); + + Assert.NotNull(_car1.WaitForAck()); + Assert.NotNull(_car2.WaitForAck()); + + Assert.Single(_car1.Reports); + Assert.Single(_car2.Reports); + Assert.Equal((ushort)11, _car1.Reports[0].Node.Node); + Assert.Equal((ushort)22, _car2.Reports[0].Node.Node); + Assert.Equal((ulong)201, _car1.Reports[0].Task); + Assert.Equal((ulong)202, _car2.Reports[0].Task); + } + + [Fact] + public void Hub_IgnoresUnregisteredCarCode() + { + SendState(99, 2, 30, 0); + Thread.Sleep(300); + Assert.Empty(_car1.Reports); + Assert.Empty(_car2.Reports); + } + + public void Dispose() + { + Fass2UdpHub.Unregister(_car1); + Fass2UdpHub.Unregister(_car2); + Fass2UdpHub.Stop(); + _car1.Dispose(); + _car2.Dispose(); + } + + private void SendState(ushort carCode, byte state, ushort nodeId, ulong taskId) + { + var frame = Fass2TestFrameBuilder.BuildStateFrame(carCode, state, nodeId, taskId); + using var sender = new UdpClient(); + sender.Send(frame, frame.Length, new IPEndPoint(IPAddress.Loopback, _hubPort)); + Thread.Sleep(80); + } + + private static int AllocateUdpPort() + { + var listener = new TcpListener(IPAddress.Loopback, 0); + listener.Start(); + var port = ((IPEndPoint)listener.LocalEndpoint).Port; + listener.Stop(); + return port; + } + } +} diff --git a/StandardScene.Magnetic.Tests/StandardScene.Magnetic.Tests.csproj b/StandardScene.Magnetic.Tests/StandardScene.Magnetic.Tests.csproj new file mode 100644 index 0000000..6b90582 --- /dev/null +++ b/StandardScene.Magnetic.Tests/StandardScene.Magnetic.Tests.csproj @@ -0,0 +1,27 @@ + + + + net8.0-windows + StandardScene.Magnetic.Tests + StandardScene.Magnetic.Tests + false + disable + disable + latest + + + + + + + all + runtime; build; native; contentfiles; analyzers; buildtransitive + + + + + + + + + diff --git a/StandardScene.Magnetic.Tests/Tasking/Fass2ControlAreaGateTests.cs b/StandardScene.Magnetic.Tests/Tasking/Fass2ControlAreaGateTests.cs new file mode 100644 index 0000000..196efff --- /dev/null +++ b/StandardScene.Magnetic.Tests/Tasking/Fass2ControlAreaGateTests.cs @@ -0,0 +1,164 @@ +using StandardScene.Magnetic.Protocol; +using StandardScene.Magnetic.Tasking; +using System.Collections.Generic; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Tasking +{ + public class Fass2ControlAreaGateTests + { + [Fact] + public void Resolve_WithoutArea_UsesCurrentSite() + { + var info = Fass2ControlAreaGate.Resolve(new Fass2SiteActionData(), 17); + + Assert.Equal(new[] { 17 }, info.AreaSiteIds); + Assert.Equal("17", info.AreaId); + Assert.Equal(1, info.Capacity); + Assert.Equal(Fass2TaskBuilder.StartStopControlStart, info.ReleaseStartStop); + } + + [Fact] + public void Resolve_ParsesSiteArray_AndReleasePass() + { + var info = Fass2ControlAreaGate.Resolve(new Fass2SiteActionData + { + ControlArea = "[10, 11, 12]", + ControlCapacity = 2, + ControlReleaseStartStop = Fass2TaskBuilder.StartStopPass + }, 8); + + Assert.Equal(new[] { 10, 11, 12 }, info.AreaSiteIds); + Assert.Equal("10,11,12", info.AreaId); + Assert.Equal(2, info.Capacity); + Assert.Equal(Fass2TaskBuilder.StartStopPass, info.ReleaseStartStop); + } + + [Fact] + public void ParseSiteIds_CommaAndSemicolon() + { + var ids = Fass2ControlAreaGate.ParseSiteIds("10;11|12", 8); + Assert.Equal(new[] { 10, 11, 12 }, ids); + } + + [Fact] + public void LimitWindow_StopsAtFirstUnreleasedControlStop() + { + var nodes = new[] + { + Node(1, Fass2TaskBuilder.StartStopPass), + Node(2, Fass2TaskBuilder.StartStopControlStop), + Node(3, Fass2TaskBuilder.StartStopPass), + Node(4, Fass2TaskBuilder.StartStopPass) + }; + + var count = Fass2ControlAreaGate.LimitWindowCount(nodes, 0, 4, releasedIndex: -1); + + Assert.Equal(2, count); + } + + [Fact] + public void LimitWindow_AfterRelease_IncludesFollowingNodes() + { + var nodes = new[] + { + Node(1, Fass2TaskBuilder.StartStopPass), + Node(2, Fass2TaskBuilder.StartStopControlStart), + Node(3, Fass2TaskBuilder.StartStopPass) + }; + + var count = Fass2ControlAreaGate.LimitWindowCount(nodes, 1, 3, releasedIndex: 1); + + Assert.Equal(2, count); + } + + [Fact] + public void Evaluate_ExcludesSelf_AllowsFirstCar() + { + var area = Area(1, 10, 11); + var occupancies = new List + { + new Fass2ControlOccupancy(1, new[] { 10, 11 }) + }; + + var check = Fass2ControlAreaGate.Evaluate(1, area, occupancies); + + Assert.True(check.CanRelease); + Assert.Equal(0, check.OthersInArea); + } + + [Fact] + public void Evaluate_OtherCarOnListedSite_EqualsCapacity_Blocks() + { + var area = Area(1, 10, 11); + var occupancies = new List + { + new Fass2ControlOccupancy(1, new[] { 8 }), + new Fass2ControlOccupancy(2, new[] { 11 }) + }; + + var check = Fass2ControlAreaGate.Evaluate(1, area, occupancies); + + Assert.False(check.CanRelease); + Assert.Equal(1, check.OthersInArea); + Assert.Contains("capacity=1", check.Reason); + } + + [Fact] + public void Evaluate_OtherCarOutsideList_DoesNotBlock() + { + var area = Area(1, 10, 11); + var occupancies = new List + { + new Fass2ControlOccupancy(2, new[] { 99 }) + }; + + var check = Fass2ControlAreaGate.Evaluate(1, area, occupancies); + + Assert.True(check.CanRelease); + Assert.Equal(0, check.OthersInArea); + } + + [Fact] + public void Evaluate_CapacityTwo_AllowsSecondCar() + { + var area = Area(2, 10, 11); + var occupancies = new List + { + new Fass2ControlOccupancy(1, new[] { 10 }), + new Fass2ControlOccupancy(2, new[] { 11 }) + }; + + var check = Fass2ControlAreaGate.Evaluate(2, area, occupancies); + + Assert.True(check.CanRelease); + Assert.Equal(1, check.OthersInArea); + } + + [Fact] + public void ActionResolver_ControlStop_IsNeverComplete() + { + var expected = Node(5, Fass2TaskBuilder.StartStopControlStop); + var actual = Node(5, Fass2TaskBuilder.StartStopControlStop); + + Assert.True(Fass2ActionResolver.RequiresActionWait(expected)); + Assert.False(Fass2ActionResolver.IsStationActionComplete(expected, actual, vehicleState: 2)); + } + + private static Fass2ControlAreaInfo Area(int capacity, params int[] siteIds) + { + return new Fass2ControlAreaInfo + { + AreaSiteIds = siteIds, + AreaId = string.Join(",", siteIds), + Capacity = capacity, + ReleaseStartStop = 11 + }; + } + + private static Fass2NodeMessage Node(ushort id, byte startStop) + { + return new Fass2NodeMessage { Node = id, StartStop = startStop }; + } + } +} diff --git a/StandardScene.Magnetic.Tests/Tasking/Fass2ControlStopStateMachineTests.cs b/StandardScene.Magnetic.Tests/Tasking/Fass2ControlStopStateMachineTests.cs new file mode 100644 index 0000000..273d00b --- /dev/null +++ b/StandardScene.Magnetic.Tests/Tasking/Fass2ControlStopStateMachineTests.cs @@ -0,0 +1,102 @@ +using StandardScene.Magnetic.Protocol; +using StandardScene.Magnetic.Tasking; +using System.Collections.Generic; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Tasking +{ + public class Fass2ControlStopStateMachineTests + { + [Fact] + public void ControlStop_TruncatesWindow_WaitsOccupancy_ThenReleases() + { + Fass2NodeMessage[] lastWindow = null; + Fass2NodeMessage lastPatch = null; + var canRelease = false; + var nodes = new List + { + new Fass2NodeMessage { Node = 1, StartStop = Fass2TaskBuilder.StartStopPass, Distance = 100 }, + new Fass2NodeMessage { Node = 2, StartStop = Fass2TaskBuilder.StartStopControlStop }, + new Fass2NodeMessage { Node = 3, StartStop = Fass2TaskBuilder.StartStopPass } + }; + var plan = new Fass2TaskPlan + { + StartSiteId = 10, + GoalSiteId = 12, + SiteIds = new List(), + Nodes = nodes, + FieldsSignature = "keep" + }; + + var machine = new Fass2TaskStateMachine(new Fass2TaskCallbacks + { + BuildPlan = (_, __, ___) => plan, + SendNodes = (window, _) => lastWindow = window, + SendAction = (patch, _) => lastPatch = patch, + SendControl = (_, __) => { }, + ResolveNodeId = id => (ushort)id, + AllocateTaskId = () => 1UL, + AllocateActionId = () => 9UL, + CheckControlRelease = _ => new Fass2ControlReleaseCheck + { + CanRelease = canRelease, + AreaId = "DockA", + Capacity = 1, + OthersInArea = canRelease ? 0 : 1, + ReleaseStartStop = Fass2TaskBuilder.StartStopControlStart, + Reason = canRelease ? "area clear" : "area occupied" + } + }) + { + StartBeforeMove = false, + LockCount = 4, + ResendIntervalMs = 60_000, + ActionRetryIntervalMs = 0 + }; + + machine.Begin(10, 12); + machine.Tick(Report(1, moving: true, startStop: 1)); + + Assert.Equal(Fass2TaskPhase.Moving, machine.Context.Phase); + Assert.NotNull(lastWindow); + Assert.Equal(2, lastWindow.Length); + Assert.Equal(Fass2TaskBuilder.StartStopControlStop, lastWindow[1].StartStop); + + machine.Tick(Report(2, moving: false, startStop: 12)); + Assert.Equal(Fass2TaskPhase.AtStation, machine.Context.Phase); + Assert.Equal(1, machine.Context.CurrentIndex); + + lastPatch = null; + machine.Tick(Report(2, moving: false, startStop: 12)); + Assert.True(machine.Context.WaitingForRelease); + Assert.Null(lastPatch); + Assert.Equal(-1, machine.Context.ControlReleasedIndex); + + canRelease = true; + machine.Tick(Report(2, moving: false, startStop: 12)); + Assert.NotNull(lastPatch); + Assert.Equal(Fass2TaskBuilder.StartStopControlStart, lastPatch.StartStop); + Assert.Equal(1, machine.Context.ControlReleasedIndex); + Assert.Equal(Fass2TaskBuilder.StartStopControlStart, nodes[1].StartStop); + Assert.True(lastWindow.Length >= 2); + Assert.Equal((ushort)3, lastWindow[lastWindow.Length - 1].Node); + + machine.Tick(Report(2, moving: false, startStop: 11)); + Assert.Equal(Fass2TaskPhase.AtStation, machine.Context.Phase); + Assert.Equal(1, machine.Context.CurrentIndex); + } + + private static Fass2StateReport Report(ushort node, bool moving, byte startStop) + { + return new Fass2StateReport + { + State = (byte)(moving ? 1 : 2), + Node = new Fass2NodeMessage + { + Node = node, + StartStop = startStop + } + }; + } + } +} diff --git a/StandardScene.Magnetic.Tests/Tasking/Fass2ReconnectChainSelectorTests.cs b/StandardScene.Magnetic.Tests/Tasking/Fass2ReconnectChainSelectorTests.cs new file mode 100644 index 0000000..f0eef7c --- /dev/null +++ b/StandardScene.Magnetic.Tests/Tasking/Fass2ReconnectChainSelectorTests.cs @@ -0,0 +1,83 @@ +using StandardScene.Magnetic.Tasking; +using System.Collections.Generic; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Tasking +{ + public class Fass2ReconnectChainSelectorTests + { + [Fact] + public void PreferGoal_OnRemainingPath_Wins() + { + var matches = new List + { + Match(1, start: 6, target: 10, site: 8, index: 1, remain: new[] { 8, 9, 10 }, prio: 9), + Match(2, start: 1, target: 5, site: 8, index: 3, remain: new[] { 8, 4, 5 }, prio: 1) + }; + + var ok = Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId: 5, out var selected, out var reason); + + Assert.True(ok); + Assert.Equal(5, selected.TargetSiteId); + Assert.Contains("preferGoal=5", reason); + } + + [Fact] + public void StartPoint_Beats_Priority_WhenGoalNotOnPath() + { + var matches = new List + { + Match(1, start: 8, target: 12, site: 8, index: 0, remain: new[] { 8, 12 }, prio: 1, atStart: true), + Match(2, start: 6, target: 10, site: 8, index: 1, remain: new[] { 8, 9, 10 }, prio: 9) + }; + + var ok = Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId: 99, out var selected, out _); + + Assert.True(ok); + Assert.Equal(12, selected.TargetSiteId); + Assert.True(selected.IsAtStartPoint); + } + + [Fact] + public void Ambiguous_DifferentTargets_SameScore_Fails() + { + var matches = new List + { + Match(1, start: 1, target: 5, site: 3, index: 1, remain: new[] { 3, 4, 5 }, prio: 1), + Match(2, start: 9, target: 7, site: 3, index: 1, remain: new[] { 3, 6, 7 }, prio: 1) + }; + + var ok = Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId: null, out var selected, out var reason); + + Assert.False(ok); + Assert.Null(selected); + Assert.Contains("ambiguous", reason); + } + + [Fact] + public void Empty_Fails() + { + var ok = Fass2ReconnectChainSelector.TrySelect(new List(), null, out _, out var reason); + Assert.False(ok); + Assert.Equal("no chain", reason); + } + + private static Fass2ChainMatch Match( + int taskId, int start, int target, int site, int index, int[] remain, int prio, bool atStart = false) + { + return new Fass2ChainMatch + { + TaskId = taskId, + TaskPriority = prio, + CurrentSiteId = site, + TargetSiteId = target, + IndexInPath = index, + DistanceToTarget = remain.Length > 0 ? remain.Length - 1 : 0, + IsAtStartPoint = atStart || index == 0, + IsAtEndPoint = remain.Length <= 1, + RemainingPath = remain, + FullPath = remain + }; + } + } +} diff --git a/StandardScene.Magnetic.Tests/Tasking/Fass2SimControlStopTests.cs b/StandardScene.Magnetic.Tests/Tasking/Fass2SimControlStopTests.cs new file mode 100644 index 0000000..8047eb9 --- /dev/null +++ b/StandardScene.Magnetic.Tests/Tasking/Fass2SimControlStopTests.cs @@ -0,0 +1,62 @@ +using StandardScene.Fass2Simulator; +using Xunit; + +namespace StandardScene.Magnetic.Tests.Tasking +{ + public class Fass2SimControlStopTests + { + [Fact] + public void ControlStop_StaysUntilReleasePatch() + { + var config = new Fass2SimConfig + { + InitialNode = 1, + DefaultSegmentDistance = 200, + DefaultSpeed = 1000, + AutoContinueOnPass = true, + AutoCompleteStationActions = false, + ActionDelayMs = 0 + }; + var (vehicle, motion, actions) = Fass2SimBootstrap.CreateTestStack(config); + vehicle.State = 1; + + motion.OnNodesCommand(3001, new[] + { + new Fass2SimNodeMessage { Node = 1, StartStop = 1, Distance = 200 }, + new Fass2SimNodeMessage { Node = 2, StartStop = 12 } + }); + + for (var i = 0; i < 20 && vehicle.Node != 2; i++) + { + motion.Tick(50); + actions.Tick(50); + } + + Assert.Equal((ushort)2, vehicle.Node); + Assert.Equal((byte)2, vehicle.State); + Assert.Equal((byte)12, vehicle.StartStop); + Assert.False(actions.CanLeaveStation()); + + motion.OnNodesCommand(3001, new[] + { + new Fass2SimNodeMessage { Node = 2, StartStop = 11, Distance = 200 }, + new Fass2SimNodeMessage { Node = 3, StartStop = 1 } + }); + actions.OnActionCommand(9, new Fass2SimNodeMessage + { + Node = 2, + StartStop = 11 + }); + actions.Tick(10); + motion.OnStationActionsCompleted(); + + for (var i = 0; i < 30 && vehicle.Node != 3; i++) + { + motion.Tick(50); + actions.Tick(50); + } + + Assert.Equal((ushort)3, vehicle.Node); + } + } +} diff --git a/StandardScene.Magnetic.Tests/TestHelpers/Fass2TestFrameBuilder.cs b/StandardScene.Magnetic.Tests/TestHelpers/Fass2TestFrameBuilder.cs new file mode 100644 index 0000000..942ee7e --- /dev/null +++ b/StandardScene.Magnetic.Tests/TestHelpers/Fass2TestFrameBuilder.cs @@ -0,0 +1,69 @@ +using StandardScene.Magnetic.Protocol; +using System; + +namespace StandardScene.Magnetic.Tests.TestHelpers +{ + internal static class Fass2TestFrameBuilder + { + public static byte[] BuildStateFrame( + ushort car, + byte state, + ushort nodeId, + ulong taskId = 0, + byte command = 0) + { + var frame = new byte[Fass2Protocol.StateFrameLength]; + frame[0] = Fass2Protocol.Begin; + frame[1] = command; + WriteUInt16(frame, 2, car); + WriteUInt16(frame, 4, 1200); + WriteUInt16(frame, 6, 800); + frame[28] = 100; + frame[29] = 100; + frame[36] = state; + WriteUInt64(frame, 45, taskId); + + var node = new Fass2NodeMessage + { + Node = nodeId, + StartStop = 1, + Distance = 500, + Speed = 120 + }; + Buffer.BlockCopy(node.ToBytes(), 0, frame, 53, 25); + + frame[98] = Fass2Protocol.Xor(frame, 1, 97); + frame[99] = Fass2Protocol.End; + return frame; + } + + public static string ToHex(byte[] bytes) + { + if (bytes == null) + { + return string.Empty; + } + + var parts = new string[bytes.Length]; + for (var i = 0; i < bytes.Length; i++) + { + parts[i] = bytes[i].ToString("X2"); + } + + return string.Join(" ", parts); + } + + private static void WriteUInt16(byte[] buffer, int offset, ushort value) + { + var bytes = BitConverter.GetBytes(value); + buffer[offset] = bytes[0]; + buffer[offset + 1] = bytes[1]; + } + + private static void WriteUInt64(byte[] buffer, int offset, ulong value) + { + var bytes = BitConverter.GetBytes(value); + Buffer.BlockCopy(bytes, 0, buffer, offset, 8); + } + } +} diff --git a/StandardScene.Magnetic.Tests/TestHelpers/TestUdpCar.cs b/StandardScene.Magnetic.Tests/TestHelpers/TestUdpCar.cs new file mode 100644 index 0000000..64dc4e6 --- /dev/null +++ b/StandardScene.Magnetic.Tests/TestHelpers/TestUdpCar.cs @@ -0,0 +1,64 @@ +using StandardScene.Magnetic.Protocol; +using System; +using System.Collections.Generic; +using System.Net; +using System.Net.Sockets; +using System.Threading; + +namespace StandardScene.Magnetic.Tests.TestHelpers +{ + internal sealed class TestUdpCar : IFass2UdpCar, IDisposable + { + private readonly UdpClient _ackListener; + private readonly object _syncRoot = new object(); + + public TestUdpCar(ushort vehicleCode, int remotePort) + { + VehicleCode = vehicleCode; + RemotePort = remotePort; + _ackListener = new UdpClient(new IPEndPoint(IPAddress.Loopback, remotePort)); + _ackListener.Client.ReceiveTimeout = 500; + } + + public ushort VehicleCode { get; } + + public string RemoteAddress => "127.0.0.1"; + + public int RemotePort { get; } + + public List Reports { get; } = new List(); + + public void OnUdpStateReceived(Fass2StateReport report) + { + lock (_syncRoot) + { + Reports.Add(report); + } + } + + public byte[] WaitForAck(int timeoutMs = 2000) + { + var deadline = Environment.TickCount64 + timeoutMs; + while (Environment.TickCount64 < deadline) + { + try + { + var remote = new IPEndPoint(IPAddress.Any, 0); + return _ackListener.Receive(ref remote); + } + catch (SocketException) + { + Thread.Sleep(20); + } + } + + return null; + } + + public void Dispose() + { + _ackListener?.Close(); + _ackListener?.Dispose(); + } + } +} diff --git a/StandardScene.Magnetic/CarTypes/MagFass2Car.cs b/StandardScene.Magnetic/CarTypes/Mag2Car.cs similarity index 68% rename from StandardScene.Magnetic/CarTypes/MagFass2Car.cs rename to StandardScene.Magnetic/CarTypes/Mag2Car.cs index 6efc77a..b0c633b 100644 --- a/StandardScene.Magnetic/CarTypes/MagFass2Car.cs +++ b/StandardScene.Magnetic/CarTypes/Mag2Car.cs @@ -31,12 +31,13 @@ namespace StandardScene.CarTypes /// [TemplateTrackCoderSettings( priority = 1, - templateString = "agv.MagFass2Go(${src.id},${dst.id},${track.Speed});", + templateString = "agv.Mag2Go(${src.id},${dst.id},${track.Speed});", + blockVerb = "true", trackFields = typeof(BasicTrackFields))] - [CarType(Name = "MagFass2Car")] - [I18N.DocumentTranslation(Name = "MagFass2Car", locale = "en")] + [CarType(Name = "Mag2Car")] + [I18N.DocumentTranslation(Name = "Mag2Car", locale = "en")] [EnvelopConfig(lengthX = 1200, lengthY = 800, centerX = 0, centerY = 0)] - public class MagFass2Car : GhostCar, IFass2UdpCar, IFass2LoopCar + public class Mag2Car : GhostCar, IFass2UdpCar, IFass2LoopCar { private readonly object _syncRoot = new object(); private TcpClient _persistentClient; @@ -47,7 +48,13 @@ namespace StandardScene.CarTypes private long _nextTaskId = 1; private long _nextActionId = 1; private Fass2TaskStateMachine _taskStateMachine; + private Fass2TrafficLocker _trafficLocker; private bool _taskRestoreAttempted; + private bool _wasOffline; + private int _reconnectWaitSiteId = -1; + private int _lastOffPathHandledSiteId = -1; + private ushort _lastMappedNode; + private ushort _lastMappedDistance; [FieldMember] public Fass2CommMode CommMode = Fass2CommMode.Udp; [FieldMember] public int Port = 5000; @@ -60,6 +67,8 @@ namespace StandardScene.CarTypes [FieldMember] public int UdpOfflineTimeoutMs = 15000; [FieldMember] public int MoveTimeoutSeconds = 120; [FieldMember] public int LockCount = 4; + [FieldMember] public bool EnableTrafficControl = true; + [FieldMember] public bool VerifyTrajectoryFieldsOnStation = false; [FieldMember] public int TaskResendIntervalMs = 500; [FieldMember] public int ActionRetryIntervalMs = 1000; [FieldMember] public bool EnableTaskStateMachine = true; @@ -98,24 +107,25 @@ namespace StandardScene.CarTypes { _lastUdpReportTime = DateTime.Now; ApplyReport(report); + TryRecoverAfterReconnectIfNeeded(report); SyncUdpOnlineTags(true); } TickTaskStateMachine(report); } - public new static async Task Create() + public new static async Task Create() { - var car = new MagFass2Car + var car = new Mag2Car { lstatus = "连接中", address = "127.0.0.1", - name = "MagFass2Car", + name = "Mag2Car", haveCoordination = true, speed = 1 }; car.EnsureTaskStateMachine(); - MagFass2CarFileLogger.Configure(car.LogDirectory, car.EnableFileLog); + Mag2CarFileLogger.Configure(car.LogDirectory, car.EnableFileLog); return car; } @@ -176,6 +186,7 @@ namespace StandardScene.CarTypes else { lstatus = "离线"; + _wasOffline = true; SyncUdpOnlineTags(false); DetailLog($"keepAlive udp waiting, listen={ListenPort}, remote={address}:{Port}"); } @@ -183,6 +194,7 @@ namespace StandardScene.CarTypes catch (Exception ex) { lstatus = "离线"; + _wasOffline = true; SyncUdpOnlineTags(false); DetailLog($"keepAlive udp failed: {ex.Message}"); } @@ -202,6 +214,7 @@ namespace StandardScene.CarTypes lstatus = IsBenignSocketFault(ex) ? lstatus : "离线"; if (!IsBenignSocketFault(ex)) { + _wasOffline = true; DetailLog($"keepAlive failed: {ex.Message}"); } } @@ -215,7 +228,7 @@ namespace StandardScene.CarTypes { if (_running) { - throw new InvalidOperationException($"MagFass2Car {id} already running script"); + throw new InvalidOperationException($"Mag2Car {id} already running script"); } if (ShouldDriveLoopGoalAsTrackedTask(script)) @@ -224,13 +237,13 @@ namespace StandardScene.CarTypes try { DetailLog($"loop goal drive intercept, goal={GoalSiteId}, scriptLen={(script ?? string.Empty).Length}"); - PrepareForLoopGoalDrive(); - await ExecuteGoalTaskAsync(skipGoalLock: true); + await ExecuteGoalTaskAsync(); DetailLog("loop goal drive completed"); } catch (Exception ex) { DetailLog($"loop goal drive failed: {ExceptionFormatter.FormatEx(ex)}"); + RecoverAfterGoalDriveFailure(); throw; } finally @@ -245,7 +258,7 @@ namespace StandardScene.CarTypes try { DetailLog($"script begin, length={(script ?? string.Empty).Length}"); - var agv = new MagFass2CarInterface(id); + var agv = new Mag2CarInterface(id); var tcs = new TaskCompletionSource(); new Thread(() => { @@ -283,19 +296,19 @@ namespace StandardScene.CarTypes } [MethodMember(Name = "启动", Description = "发送启动指令(0x01)")] - public void StartMagFass2Car() + public void StartMag2Car() { - SendControl(Fass2Protocol.CmdStart, (ushort)Math.Round(th)); + SendControl(Fass2Protocol.CmdStart, 0); } [MethodMember(Name = "停止", Description = "发送停止指令(0x02)")] - public void StopMagFass2Car() + public void StopMag2Car() { SendControl(Fass2Protocol.CmdStop, 0); } [MethodMember(Name = "急停", Description = "发送急停指令(0x03)")] - public void EmergencyStopMagFass2Car() + public void EmergencyStopMag2Car() { SendControl(Fass2Protocol.CmdEmergencyStop, 0); } @@ -315,14 +328,14 @@ namespace StandardScene.CarTypes public void EmergencyStop(string reason) { - Diagnosis.Post($"car{name}:MagFass2Car EmergencyStop {reason}"); + Diagnosis.Post($"car{name}:Mag2Car EmergencyStop {reason}"); SendControl(Fass2Protocol.CmdEmergencyStop, 0); } public void EmergencyRelease() { - Diagnosis.Post($"car{name}:MagFass2Car EmergencyRelease"); - SendControl(Fass2Protocol.CmdStart, (ushort)Math.Round(th)); + Diagnosis.Post($"car{name}:Mag2Car EmergencyRelease"); + SendControl(Fass2Protocol.CmdStart, 0); } [MethodMember(Name = "重置UDP监听", Description = "重新注册 UDP 会话（PCB 模式）")] @@ -361,6 +374,9 @@ namespace StandardScene.CarTypes _running = false; _lastSyncedTrafficSiteId = -1; _taskRestoreAttempted = false; + _wasOffline = false; + _reconnectWaitSiteId = -1; + _lastOffPathHandledSiteId = -1; if (_taskStateMachine != null && !_taskStateMachine.IsIdle) { @@ -417,6 +433,63 @@ namespace StandardScene.CarTypes Fass2TaskPersistence.Clear(tags); } + /// + /// 任务失败/超时后清 pending、复位本站交管锁，去掉 occupied，保留 goalSite 供环线重派。 + /// + private void RecoverAfterGoalDriveFailure() + { + try + { + if (_taskStateMachine != null && !_taskStateMachine.IsIdle) + { + _taskStateMachine.Cancel("recover after failure"); + } + } + catch + { + // ignore + } + + try + { + ClearTaskPersistence(); + Commons.DeleteTag(tags, "occupied"); + + var site = ResolveSite(LastReport.Node.Node); + if (site == null && siteID > 0) + { + site = SimpleLib.GetSite(siteID); + } + + if (site == null && GetLastSite() > 0) + { + site = SimpleLib.GetSite(GetLastSite()); + } + + if (site != null) + { + TrafficReset(site, true, strict: false); + siteID = site.id; + _lastSyncedTrafficSiteId = site.id; + haveCoordination = true; + } + else + { + status.pendingLocks = Array.Empty(); + status.seqScope = Array.Empty(); + status.seqPtr = 0; + } + + SyncUdpOnlineTags(IsUdpSessionOnline()); + DetailLog( + $"goal drive recover done, keepGoal={GoalSiteId}, site={(site?.id.ToString() ?? "-")}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]"); + } + catch (Exception ex) + { + DetailLog($"goal drive recover failed: {ex.Message}"); + } + } + private bool IsUdpSessionOnline() { return CommMode == Fass2CommMode.Udp && @@ -433,7 +506,7 @@ namespace StandardScene.CarTypes (DateTime.Now - _lastUdpReportTime).TotalMilliseconds > UdpOfflineTimeoutMs) { throw new TimeoutException( - $"MagFass2Car {name}({id}) UDP state timeout ({UdpOfflineTimeoutMs}ms), listen={ListenPort}"); + $"Mag2Car {name}({id}) UDP state timeout ({UdpOfflineTimeoutMs}ms), listen={ListenPort}"); } return LastReport; @@ -443,6 +516,7 @@ namespace StandardScene.CarTypes var response = SendRequest(request, Fass2Protocol.StateFrameLength, "Query(0x00)"); var report = Fass2Protocol.ParseState(response); ApplyReport(report); + TryRecoverAfterReconnectIfNeeded(report); return report; } @@ -467,7 +541,7 @@ namespace StandardScene.CarTypes public Fass2TaskPlan BuildTaskPlan(int startSiteId, int goalSiteId, double defaultSpeed = -1) { - return Fass2TaskBuilder.BuildPath(startSiteId, goalSiteId, CreateTaskBuildOptions(defaultSpeed), ResolveNodeId); + return Fass2TaskBuilder.BuildPath(startSiteId, goalSiteId, CreateTaskBuildOptions(defaultSpeed), ResolveNodeId, this); } public void SendTaskPlan(Fass2TaskPlan plan) @@ -494,7 +568,7 @@ namespace StandardScene.CarTypes _taskStateMachine.ResendIntervalMs = TaskResendIntervalMs; _taskStateMachine.ActionRetryIntervalMs = ActionRetryIntervalMs; _taskStateMachine.StartBeforeMove = StartBeforeMove; - _taskStateMachine.HeadingAngle = (ushort)Math.Round(th); + Fass2ActionResolver.VerifyTrajectoryFields = VerifyTrajectoryFieldsOnStation; _taskStateMachine.Begin(startSiteId, goalSiteId, defaultSpeed); var tick = _taskStateMachine.Tick(LastReport); @@ -520,11 +594,11 @@ namespace StandardScene.CarTypes async Task IFass2LoopCar.ExecuteGoalTaskAsync(int? goalSiteId, double defaultSpeed, CancellationToken cancellationToken) { - await ExecuteGoalTaskAsync(goalSiteId, defaultSpeed, cancellationToken, skipGoalLock: false); + await ExecuteGoalTaskAsync(goalSiteId, defaultSpeed, cancellationToken); } public async Task ExecuteGoalTaskAsync(int? goalSiteId = null, double defaultSpeed = -1, - CancellationToken cancellationToken = default, bool skipGoalLock = false) + CancellationToken cancellationToken = default) { if (!EnableTaskStateMachine) { @@ -534,13 +608,13 @@ namespace StandardScene.CarTypes var goalId = goalSiteId ?? GoalSiteId; if (goalId == null || goalId.Value <= 0) { - throw new InvalidOperationException("MagFass2Car goalSite is not set"); + throw new InvalidOperationException("Mag2Car goalSite is not set"); } var startId = ResolvePhysicalSiteId(); if (startId <= 0) { - throw new InvalidOperationException($"MagFass2Car {name}({id}) cannot resolve start site for loop task"); + throw new InvalidOperationException($"Mag2Car {name}({id}) cannot resolve start site for loop task"); } var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1; @@ -549,56 +623,11 @@ namespace StandardScene.CarTypes DetailLog($"ExecuteGoalTaskAsync normalize start, holding={lockedSite}, reportStart={startId}"); } - DetailLog($"ExecuteGoalTaskAsync start={startId}, goal={goalId.Value}, speed={defaultSpeed}, skipGoalLock={skipGoalLock}"); + DetailLog($"ExecuteGoalTaskAsync start={startId}, goal={goalId.Value}, speed={defaultSpeed}, traffic={EnableTrafficControl}"); - var goalLocked = false; - if (!skipGoalLock) - { - while (!TrafficControl.TryLock(this, goalId.Value)) - { - cancellationToken.ThrowIfCancellationRequested(); - await Task.Delay(50, cancellationToken); - } - - goalLocked = true; - } - - try - { - BeginTrackedTask(startId, goalId.Value, defaultSpeed); - await WaitTrackedTaskAsync(MoveTimeoutSeconds * 1000); - TaskCompleted?.Invoke(goalId.Value); - } - finally - { - if (goalLocked) - { - TrafficControl.Leave(this, goalId.Value); - } - } - } - - /// - /// Loop GoSite 在 Compile 前会通过 FindRoute 写入 seqScope/pendingLocks； - /// 全程任务改由 FASS2 状态机推进，需先清掉这段交管残留，避免 TryLock 与顺序锁冲突。 - /// - private void PrepareForLoopGoalDrive() - { - var startSiteId = ResolvePhysicalSiteId(); - var site = startSiteId > 0 ? SimpleLib.GetSite(startSiteId) : null; - - DetailLog( - $"loop goal prepare before reset, node={LastReport.Node.Node}, start={startSiteId}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]"); - - if (site != null) - { - TrafficReset(site, true, strict: false); - siteID = site.id; - _lastSyncedTrafficSiteId = site.id; - } - - DetailLog( - $"loop goal prepare after reset, start={startSiteId}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]"); + BeginTrackedTask(startId, goalId.Value, defaultSpeed); + await WaitTrackedTaskAsync(MoveTimeoutSeconds * 1000); + TaskCompleted?.Invoke(goalId.Value); } /// @@ -631,7 +660,7 @@ namespace StandardScene.CarTypes } return !string.IsNullOrEmpty(script) && - script.Contains("MagFass2Go", StringComparison.Ordinal); + script.Contains("Mag2Go", StringComparison.Ordinal); } private bool TryGetGoalSiteId(out int goalSiteId) @@ -653,6 +682,8 @@ namespace StandardScene.CarTypes return; } + _trafficLocker = new Fass2TrafficLocker(this, message => DetailLog(message)); + _taskStateMachine = new Fass2TaskStateMachine(new Fass2TaskCallbacks { BuildPlan = BuildTaskPlan, @@ -665,7 +696,77 @@ namespace StandardScene.CarTypes AllocateActionId = () => (ulong)Interlocked.Increment(ref _nextActionId), Persist = PersistTaskContext, ClearPersisted = ClearTaskPersistence, - Log = message => DetailLog($"task-sm {message}") + Log = message => DetailLog($"task-sm {message}"), + PrepareTraffic = (siteIds, fromIndex) => + { + if (!EnableTrafficControl) + { + return; + } + + _trafficLocker.PrepareSequence(siteIds, fromIndex); + }, + EnsureTrafficWindow = (siteIds, fromIndex, wantWindow) => + { + if (!EnableTrafficControl) + { + return wantWindow; + } + + return _trafficLocker.EnsureWindowLocked(siteIds, fromIndex, wantWindow); + }, + LeaveTraffic = siteId => + { + if (!EnableTrafficControl) + { + return; + } + + _trafficLocker.LeavePassed(siteId); + }, + ReleaseTrafficBehind = (siteIds, keepFromIndex) => + { + if (!EnableTrafficControl) + { + return; + } + + _trafficLocker.ReleaseLocksBehind(siteIds, keepFromIndex); + }, + RebaseTraffic = (siteIds, fromIndex) => + { + if (!EnableTrafficControl) + { + return; + } + + _trafficLocker.RebaseFrom(siteIds, fromIndex, LockCount); + }, + FinalizeTraffic = siteId => + { + if (!EnableTrafficControl) + { + return; + } + + _trafficLocker.FinalizeAtSite(siteId); + }, + CheckControlRelease = siteId => + { + try + { + return Fass2ControlAreaOccupancy.Check(this, siteId); + } + catch (Exception ex) + { + DetailLog($"control occupancy failed: site={siteId}, {ex.Message}"); + return new Fass2ControlReleaseCheck + { + CanRelease = false, + Reason = ex.Message + }; + } + } }); } @@ -708,6 +809,238 @@ namespace StandardScene.CarTypes } } + private void TryRecoverAfterReconnectIfNeeded(Fass2StateReport report) + { + var site = report?.Node != null ? ResolveSite(report.Node.Node) : null; + if (site == null) + { + if (_wasOffline || _reconnectWaitSiteId > 0) + { + DetailLog($"reconnect skip, unmapped node={report?.Node.Node}"); + } + + return; + } + + EnsureTaskStateMachine(); + var running = _taskStateMachine != null && _taskStateMachine.IsRunning; + var onRoute = running && _taskStateMachine.TryFindIndexOnRoute(site.id, out _); + var offlineRecover = _wasOffline || _reconnectWaitSiteId > 0; + var offPathRecover = running && !onRoute && _lastOffPathHandledSiteId != site.id; + + if (!offlineRecover && !offPathRecover) + { + return; + } + + if (_reconnectWaitSiteId > 0 && _reconnectWaitSiteId != site.id) + { + _reconnectWaitSiteId = -1; + } + + _wasOffline = false; + + var holding = status.holdingLocks == null || status.holdingLocks.Length == 0 + ? string.Empty + : string.Join(",", status.holdingLocks); + var routeText = FormatRouteSites(); + DetailLog( + $"reconnect site={site.id}, onRoute={onRoute}, offPath={offPathRecover}, taskPhase={TaskPhase}, holding=[{holding}], goal={GoalSiteId?.ToString() ?? "-"}, route=[{routeText}]"); + + if (!running) + { + _reconnectWaitSiteId = -1; + DetailLog("reconnect idle, keep SyncTrafficFromReport"); + return; + } + + if (!TryClaimReportedSite(site)) + { + _reconnectWaitSiteId = site.id; + DetailLog($"reconnect wait, site={site.id} held by another car"); + return; + } + + _reconnectWaitSiteId = -1; + + try + { + if (onRoute) + { + if (_taskStateMachine.IsAtRouteEnd(site.id)) + { + TrafficResetToReportedSite(site); + _taskStateMachine.TryCompleteAtSite(site.id); + DetailLog($"reconnect rebase complete at end site={site.id}"); + return; + } + + if (_taskStateMachine.RebaseToSite(site.id)) + { + _lastSyncedTrafficSiteId = site.id; + DetailLog($"reconnect rebase site={site.id}"); + } + + return; + } + + _lastOffPathHandledSiteId = site.id; + RecoverOffOriginalRoute(site); + } + catch (Exception ex) + { + DetailLog($"reconnect recover failed: {ex.Message}"); + } + } + + /// + /// 上报站不在当前任务 Plan/RouteSiteIds 上：按 tasklist 归属链路重下，不再用原 goal 续跑。 + /// + private void RecoverOffOriginalRoute(Site site) + { + var originalGoal = _taskStateMachine.Context.GoalSiteId > 0 + ? _taskStateMachine.Context.GoalSiteId + : GoalSiteId ?? 0; + + var matches = Mag2LoopMission.FindChainsContaining(site.id); + if (originalGoal > 0 && matches.Count > 0) + { + matches = matches.Where(m => m != null && m.TargetSiteId != originalGoal).ToList(); + } + + if (Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId: null, out var chain, out var reason) + && chain != null) + { + DetailLog($"reconnect off-path chain={chain}, reason={reason}, dropGoal={originalGoal}"); + if (chain.IsAtEndPoint || chain.TargetSiteId == site.id) + { + HandOffToAutoLoop(site); + _taskStateMachine.TryCompleteAtSite(site.id); + DetailLog($"reconnect chain end site={site.id}, goal={chain.TargetSiteId}"); + return; + } + + SetGoalSiteTag(chain.TargetSiteId); + if (_taskStateMachine.RestartFromSite(site.id, chain.TargetSiteId)) + { + TrafficResetToReportedSite(site); + _lastSyncedTrafficSiteId = site.id; + DetailLog($"reconnect restart start={site.id}, goal={chain.TargetSiteId}"); + return; + } + } + + DetailLog( + $"reconnect off-path unmatched site={site.id}, dropGoal={originalGoal}, matches={matches.Count}, {reason ?? "no chain"}"); + + if (!EnableLoopTaskDrive && originalGoal > 0 && originalGoal != site.id) + { + try + { + var probe = BuildTaskPlan(site.id, originalGoal); + if (probe?.SiteIds != null && probe.SiteIds.Count > 0) + { + SetGoalSiteTag(originalGoal); + if (_taskStateMachine.RestartFromSite(site.id, originalGoal)) + { + TrafficResetToReportedSite(site); + _lastSyncedTrafficSiteId = site.id; + DetailLog($"reconnect restart keepGoal (non-loop) start={site.id}, goal={originalGoal}"); + return; + } + } + } + catch (Exception ex) + { + DetailLog($"reconnect keepGoal plan failed: {ex.Message}"); + } + } + + DetailLog($"reconnect unmatched site={site.id}, handoff AutoLoop"); + HandOffToAutoLoop(site); + if (!_taskStateMachine.IsIdle) + { + _taskStateMachine.Cancel("reconnect unmatched site"); + } + } + + private string FormatRouteSites() + { + var sites = _taskStateMachine?.Context?.Plan?.SiteIds; + if (sites == null || sites.Count == 0) + { + sites = _taskStateMachine?.Context?.RouteSiteIds; + } + + if (sites == null || sites.Count == 0) + { + return string.Empty; + } + + return string.Join(",", sites); + } + + private bool TryClaimReportedSite(Site site) + { + if (site == null) + { + return false; + } + + var holder = FindOtherCarHoldingSite(site.id); + if (holder == null) + { + return true; + } + + if (TryReleaseStaleHolder(holder, site.id)) + { + DetailLog($"reconnect released stale holder car={holder.id}, site={site.id}"); + return true; + } + + return false; + } + + private void TrafficResetToReportedSite(Site site) + { + if (site == null) + { + return; + } + + try + { + TrafficReset(site, true, strict: false); + siteID = site.id; + _lastSyncedTrafficSiteId = site.id; + haveCoordination = true; + } + catch (Exception ex) + { + DetailLog($"reconnect TrafficReset failed: site={site.id}, {ex.Message}"); + } + } + + private void HandOffToAutoLoop(Site site) + { + Commons.DeleteTag(tags, "occupied"); + Commons.DeleteTag(tags, "goalSite"); + Commons.DeleteTag(tags, "loopAssigned"); + ClearTaskPersistence(); + TrafficResetToReportedSite(site); + } + + private void SetGoalSiteTag(int goalSiteId) + { + if (tags == null || goalSiteId <= 0) + { + return; + } + + Commons.AddOrUpdateTag(tags, "goalSite", goalSiteId.ToString()); + } + private void PersistTaskContext(Fass2TaskContext context) { if (tags == null || context == null) @@ -758,7 +1091,7 @@ namespace StandardScene.CarTypes return new Fass2TaskBuildOptions { UseTagValueAsNode = UseTagValueAsNode, - DefaultSpeed = defaultSpeed > 0 ? defaultSpeed : 0.2, + DefaultSpeed = defaultSpeed > 0 ? defaultSpeed : 12, CarSpeed = speed }; } @@ -811,7 +1144,7 @@ namespace StandardScene.CarTypes { if (string.IsNullOrWhiteSpace(address)) { - throw new InvalidOperationException("MagFass2Car address is empty for UDP mode"); + throw new InvalidOperationException("Mag2Car address is empty for UDP mode"); } if (!_udpRegistered || _registeredVehicleCode != VehicleCode) @@ -941,7 +1274,7 @@ namespace StandardScene.CarTypes { if (string.IsNullOrWhiteSpace(address)) { - throw new InvalidOperationException("MagFass2Car address is empty"); + throw new InvalidOperationException("Mag2Car address is empty"); } using var cts = new CancellationTokenSource(ConnectTimeoutMs); @@ -1028,7 +1361,7 @@ namespace StandardScene.CarTypes /// internal Fass2NodeMessage[] BuildMoveNodes(int srcId, int dstId, double trackSpeed = -1) { - return Fass2TaskBuilder.BuildSegment(srcId, dstId, CreateTaskBuildOptions(trackSpeed), ResolveNodeId); + return Fass2TaskBuilder.BuildSegment(srcId, dstId, CreateTaskBuildOptions(trackSpeed), ResolveNodeId, this); } private void WaitUntilArrived(ushort targetNode, int timeoutMs) @@ -1047,7 +1380,7 @@ namespace StandardScene.CarTypes } throw new TimeoutException( - $"MagFass2Car {name}({id}) move timeout, target node={targetNode}, current={LastReport.Node.Node}, state={Fass2Protocol.StateText(LastReport.State)}"); + $"Mag2Car {name}({id}) move timeout, target node={targetNode}, current={LastReport.Node.Node}, state={Fass2Protocol.StateText(LastReport.State)}"); } private void ApplyReport(Fass2StateReport report) @@ -1094,20 +1427,40 @@ namespace StandardScene.CarTypes /// /// 路段内按 Distance 在起止站点间插值，避免地图只在 node 跳变时才移动。 + /// + /// 协议行驶中 Node 一直是出发站、Distance 递增；到站才改 Node。 + /// 若中途 0xB1 重下/task 重建把 Distance 清 0，旧逻辑会把车坐标打回出发站中心 → 看起来“闪回上一站”。 + /// /// private void ApplyMapCoordinates(Site site, Fass2StateReport report) { siteID = site.id; - x = site.x; - y = site.y; - if (report.Node.Distance <= 0 || - !TryResolveMotionSegment(report.Node.Node, out var fromSite, out var toSite, out var segmentLenMm)) + var node = report.Node.Node; + var dist = report.Node.Distance; + var moving = Fass2ActionResolver.IsVehicleMoving(report.State); + + // 同站行驶中 Distance 被清零：保留当前插值坐标，不要拽回站心。 + if (dist <= 0 && moving && + _lastMappedNode == node && _lastMappedDistance > 100) { + DetailLog( + $"map coord keep (dist cleared while moving), node={node}, lastDist={_lastMappedDistance}, site={site.id}"); + _lastMappedDistance = 0; return; } - var progress = Math.Min(1.0, report.Node.Distance / Math.Max(1.0, segmentLenMm)); + if (dist <= 0 || + !TryResolveMotionSegment(node, out var fromSite, out var toSite, out var segmentLenMm)) + { + x = site.x; + y = site.y; + _lastMappedNode = node; + _lastMappedDistance = dist; + return; + } + + var progress = Math.Min(1.0, dist / Math.Max(1.0, segmentLenMm)); var dx = toSite.x - fromSite.x; var dy = toSite.y - fromSite.y; var mapLen = Math.Sqrt(dx * dx + dy * dy); @@ -1116,6 +1469,8 @@ namespace StandardScene.CarTypes { x = (float)(fromSite.x + dx * progress); y = (float)(fromSite.y + dy * progress); + _lastMappedNode = node; + _lastMappedDistance = dist; return; } @@ -1128,13 +1483,17 @@ namespace StandardScene.CarTypes { x = (float)(fromSite.x + trackDx * progress); y = (float)(fromSite.y + trackDy * progress); + _lastMappedNode = node; + _lastMappedDistance = dist; return; } } // 地图起止站点坐标重合时，用协议距离沿 X 轴做最小可视化偏移。 - x = (float)(fromSite.x + report.Node.Distance); + x = (float)(fromSite.x + dist); y = fromSite.y; + _lastMappedNode = node; + _lastMappedDistance = dist; } private bool TryResolveMotionSegment(ushort currentNode, out Site fromSite, out Site toSite, out double segmentLenMm) @@ -1169,7 +1528,8 @@ namespace StandardScene.CarTypes return false; } - segmentLenMm = plan.Nodes[i + 1].Distance; + // Distance 挂在出发站节点上（边长），不是下一站。 + segmentLenMm = plan.Nodes[i].Distance; if (segmentLenMm <= 0) { var dx = toSite.x - fromSite.x; @@ -1236,44 +1596,60 @@ namespace StandardScene.CarTypes return; } - var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1; - if (_running) + var taskRunning = _running || (_taskStateMachine?.IsRunning ?? false); + if (taskRunning) { + var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1; if (lockedSite == site.id) { _lastSyncedTrafficSiteId = site.id; - return; } - - try + else { DetailLog( - $"traffic sync during task, lock={lockedSite}, targetSite={site.id}, node={report.Node.Node}"); - TrafficReset(site, true, strict: false); - _lastSyncedTrafficSiteId = site.id; - } - catch (Exception ex) - { - DetailLog($"traffic sync during task failed: {ex.Message}"); + $"traffic sync skipped during task, lock={lockedSite}, reportSite={site.id}, node={report.Node.Node}"); } return; } - if (lockedSite == site.id) + var idleLockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1; + if (idleLockedSite == site.id) { _lastSyncedTrafficSiteId = site.id; return; } - if (_lastSyncedTrafficSiteId == site.id && lockedSite == site.id) + if (_lastSyncedTrafficSiteId == site.id && idleLockedSite == site.id) { return; } + // 双车叠在同一站时，后到车不能 TrafficReset 抢锁；离线/错站的陈旧占锁则回收后重占。 + var holder = FindOtherCarHoldingSite(site.id); + if (holder != null) + { + if (TryReleaseStaleHolder(holder, site.id)) + { + DetailLog( + $"traffic sync released stale holder car={holder.id}, site={site.id}, node={report.Node.Node}"); + } + else + { + if (_lastSyncedTrafficSiteId != -site.id) + { + DetailLog( + $"traffic sync skip, site={site.id} held by car={holder.id}, selfLock={idleLockedSite}, node={report.Node.Node}"); + _lastSyncedTrafficSiteId = -site.id; + } + + return; + } + } + try { - DetailLog($"traffic sync TrafficReset, lock={lockedSite}, targetSite={site.id}, node={report.Node.Node}"); + DetailLog($"traffic sync TrafficReset, lock={idleLockedSite}, targetSite={site.id}, node={report.Node.Node}"); TrafficReset(site, true, strict: false); _lastSyncedTrafficSiteId = site.id; } @@ -1283,6 +1659,58 @@ namespace StandardScene.CarTypes } } + private Car FindOtherCarHoldingSite(int siteId) + { + foreach (var other in SimpleLib.GetAllCars().OfType()) + { + if (other == null || other.id == id) + { + continue; + } + + if (other.status?.holdingLocks != null && + Array.IndexOf(other.status.holdingLocks, siteId) >= 0) + { + return other; + } + } + + return null; + } + + /// + /// 他车离线，或上报站点已离开，却仍占着本站锁 → 视为陈旧占锁，允许回收。 + /// + private bool TryReleaseStaleHolder(Car other, int siteId) + { + if (other == null) + { + return false; + } + + var online = other.tags?.Contains("Online") == true; + var physicalSiteId = other is Mag2Car mag2 && mag2.siteID > 0 + ? mag2.siteID + : other.GetLastSite(); + + var stale = !online || (physicalSiteId > 0 && physicalSiteId != siteId); + if (!stale) + { + return false; + } + + try + { + TrafficControl.Leave(other, siteId); + return Array.IndexOf(other.status.holdingLocks ?? Array.Empty(), siteId) < 0; + } + catch (Exception ex) + { + DetailLog($"traffic sync release stale holder failed: car={other.id}, site={siteId}, {ex.Message}"); + return false; + } + } + private Site ResolveSite(ushort node) { if (node == 0) @@ -1333,9 +1761,9 @@ namespace StandardScene.CarTypes return; } - var text = $"[MagFass2Car:{name}({id})] {DateTime.Now:HH:mm:ss.fff} {message}"; + var text = $"[Mag2Car:{name}({id})] {DateTime.Now:HH:mm:ss.fff} {message}"; AppendDebug(text); - Diagnosis.Post(text, "MagFass2Car", true); + Diagnosis.Post(text, "Mag2Car", true); } private void WriteFileLog(string message) @@ -1351,8 +1779,8 @@ namespace StandardScene.CarTypes return; } - MagFass2CarFileLogger.Configure(LogDirectory, true); - MagFass2CarFileLogger.Write(logVehicleCode, message); + Mag2CarFileLogger.Configure(LogDirectory, true); + Mag2CarFileLogger.Write(logVehicleCode, message); } private ushort ResolveLogVehicleCode() @@ -1403,13 +1831,13 @@ namespace StandardScene.CarTypes } } - internal sealed class MagFass2CarInterface : AGVInterface + internal sealed class Mag2CarInterface : AGVInterface { - private readonly MagFass2Car _car; + private readonly Mag2Car _car; - public MagFass2CarInterface(int id) + public Mag2CarInterface(int id) { - _car = (MagFass2Car)SimpleLib.GetCar(id); + _car = (Mag2Car)SimpleLib.GetCar(id); } public override bool TryLock(int siteId) @@ -1419,6 +1847,11 @@ namespace StandardScene.CarTypes throw new Exception("abandoned"); } + if (_car.status.pendingLocks.Length > 0 && _car.status.pendingLocks[0] != siteId) + { + throw new Exception("lock not according to sequence"); + } + return TrafficControl.TryLock(_car, siteId); } @@ -1427,18 +1860,12 @@ namespace StandardScene.CarTypes TrafficControl.Leave(_car, siteId); } - public void MagFass2Go(int srcId, int dstId, double speed = -1) + public void Mag2Go(int srcId, int dstId, double speed = -1) { var promise = new TaskCompletionSource(); Queue(async () => { - while (!TryLock(dstId)) - { - await Task.Delay(50); - } - - var dstNode = _car.ResolveNodeId(dstId); - _car.DetailLog($"MagFass2Go src={srcId}, dst={dstId}, dstNode={dstNode}, speed={speed}"); + _car.DetailLog($"Mag2Go src={srcId}, dst={dstId}, speed={speed}, traffic={_car.EnableTrafficControl}"); if (_car.EnableTaskStateMachine) { @@ -1449,21 +1876,20 @@ namespace StandardScene.CarTypes { if (_car.StartBeforeMove) { - _car.SendControl(Fass2Protocol.CmdStart, (ushort)Math.Round(_car.th)); + _car.SendControl(Fass2Protocol.CmdStart, 0); } var plan = _car.BuildTaskPlan(srcId, dstId, speed); _car.DetailLog( - $"MagFass2Go plan sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}"); + $"Mag2Go plan sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}"); _car.SendTaskPlan(plan); - _car.WaitUntilArrived(dstNode, _car.MoveTimeoutSeconds * 1000); + _car.WaitUntilArrived(_car.ResolveNodeId(dstId), _car.MoveTimeoutSeconds * 1000); } promise.SetResult(1); }, async () => { await promise.Task; - Leave(srcId); }); } } diff --git a/StandardScene.Magnetic/Chained/Mag2LoopMission.cs b/StandardScene.Magnetic/Chained/Mag2LoopMission.cs new file mode 100644 index 0000000..2c72a62 --- /dev/null +++ b/StandardScene.Magnetic/Chained/Mag2LoopMission.cs @@ -0,0 +1,209 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Newtonsoft.Json; +using SimpleCore; +using SimpleCore.Library; +using SimpleLite; +using SimpleLite.Props; +using SimpleLite.RCS; +using SimpleLite.RCS.CarTypes; +using StandardScene.CarTypes; +using StandardScene.Chained; +using StandardScene.Magnetic.Tasking; +using StandardScene.Model; + +namespace StandardScene +{ + /// + /// 磁导航 FASS 2.0 环线任务进程。 + /// 任务分配/触发/流量逻辑继承 Core 。 + /// 导航执行由 拦截 Loop 编译脚本（含 Mag2Go + goalSite）， + /// 改为单次全程 0xB1 状态机任务。 + /// + [MissionType(Name = "磁导航FASS2环线", editor = typeof(Mag2LoopMission))] + public class Mag2LoopMission : LoopMission + { + [JsonIgnore] public override MissionStatus status { get; set; } = new LoopMissionStatus(); + + /// + /// Mag2 空闲时可能尚未 TrafficReset（holdingLocks 为空），仍按上报站点/siteID 匹配在站车辆， + /// 否则 AutoLoop 永远找不到车、加不上 goalSite。 + /// + protected override Car FindCarArrivedAtSite(int siteId) + { + var byLock = base.FindCarArrivedAtSite(siteId); + if (byLock != null) + return byLock; + + try + { + return SimpleLib.GetAllCars() + .OfType() + .FirstOrDefault(car => + { + if (car?.tags == null || car.tags.Contains("occupied") || !car.tags.Contains("Online")) + return false; + if (car.status?.pendingLocks != null && car.status.pendingLocks.Length > 0) + return false; + if (car.status?.holdingLocks != null && car.status.holdingLocks.Length > 1) + return false; + + var physical = car.siteID > 0 ? car.siteID : car.GetLastSite(); + return physical == siteId; + }); + } + catch + { + return null; + } + } + + /// + /// 分配前尽量补上本站交管锁，保证 SelectCar/GoSite 的 GetLastSite 可用。 + /// + protected override void AssignCarToTarget(Car car, int targetSiteId) + { + if (car is Mag2Car mag2) + { + EnsureMag2HoldingCurrentSite(mag2); + } + + base.AssignCarToTarget(car, targetSiteId); + } + + /// + /// 用当前占点匹配全部 LoopTask 展开路径（不过滤 IsViaPoint）。 + /// 路径用 BFS，避免 FindRoute 污染交管锁。 + /// + public static List FindChainsContaining(int siteId) + { + var matches = new List(); + if (siteId <= 0) + { + return matches; + } + + IEnumerable missions = null; + try + { + missions = SimpleProject.proj?.Missions?.OfType(); + } + catch + { + missions = null; + } + + if (missions == null) + { + return matches; + } + + foreach (var mission in missions) + { + IReadOnlyList tasks; + try + { + tasks = mission.GetTasks(); + } + catch + { + continue; + } + + if (tasks == null) + { + continue; + } + + foreach (var task in tasks) + { + var match = TryMatchTask(task, siteId); + if (match != null) + { + matches.Add(match); + } + } + } + + return matches; + } + + public static bool TryResolveBelongingChain( + int siteId, + int? preferGoalSiteId, + out Fass2ChainMatch selected, + out string reason) + { + var matches = FindChainsContaining(siteId); + return Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId, out selected, out reason); + } + + private static Fass2ChainMatch TryMatchTask(LoopTask task, int siteId) + { + if (task == null || task.CurrentStationId <= 0 || task.TargetStationId <= 0) + { + return null; + } + + List fullPath; + try + { + fullPath = Fass2RouteHelper.GetSitesBetween(task.CurrentStationId, task.TargetStationId, null); + } + catch + { + return null; + } + + if (fullPath == null || fullPath.Count == 0) + { + return null; + } + + var index = fullPath.IndexOf(siteId); + if (index < 0) + { + return null; + } + + var remaining = fullPath.Skip(index).ToList(); + return new Fass2ChainMatch + { + TaskId = task.Id, + TaskPriority = task.Priority, + CurrentSiteId = siteId, + TargetSiteId = task.TargetStationId, + IndexInPath = index, + DistanceToTarget = Math.Max(0, fullPath.Count - index - 1), + IsAtStartPoint = index == 0, + IsAtEndPoint = index == fullPath.Count - 1, + FullPath = fullPath, + RemainingPath = remaining + }; + } + + private static void EnsureMag2HoldingCurrentSite(Mag2Car car) + { + try + { + if (car.status?.holdingLocks != null && car.status.holdingLocks.Length == 1) + return; + + var siteId = car.siteID > 0 ? car.siteID : car.GetLastSite(); + if (siteId <= 0) + return; + + var site = SimpleLib.GetSite(siteId); + if (site == null) + return; + + car.TrafficReset(site, true, strict: false); + } + catch (Exception) + { + // 交管失败时仍允许打上 goalSite，后续 GoSite/状态机会再处理 + } + } + } +} diff --git a/StandardScene.Magnetic/Chained/MagFass2LoopMission.cs b/StandardScene.Magnetic/Chained/MagFass2LoopMission.cs deleted file mode 100644 index a7efd26..0000000 --- a/StandardScene.Magnetic/Chained/MagFass2LoopMission.cs +++ /dev/null @@ -1,20 +0,0 @@ -using Newtonsoft.Json; -using SimpleLite.Props; -using SimpleLite.RCS; -using StandardScene.Chained; - -namespace StandardScene -{ - /// - /// 磁导航 FASS 2.0 环线任务进程。 - /// 任务分配/触发/流量逻辑完全继承 Core ，不修改 Core。 - /// 导航执行由 拦截 Loop 编译脚本（含 MagFass2Go + goalSite）， - /// 改为单次全程 0xB1 状态机任务。 - /// 场景内可继续使用原 ；本类型仅作磁导航 FASS2 场景标识。 - /// - [MissionType(Name = "磁导航FASS2环线", editor = typeof(MagFass2LoopMission))] - public class MagFass2LoopMission : LoopMission - { - [JsonIgnore] public override MissionStatus status { get; set; } = new LoopMissionStatus(); - } -} diff --git a/StandardScene.Magnetic/MagneticSceneProfile.cs b/StandardScene.Magnetic/MagneticSceneProfile.cs index 3e287de..9ac81c1 100644 --- a/StandardScene.Magnetic/MagneticSceneProfile.cs +++ b/StandardScene.Magnetic/MagneticSceneProfile.cs @@ -7,9 +7,9 @@ namespace StandardScene.Magnetic { /// /// scene.mag 平台画像：磁导航场景插件。 - /// 车型：（FASS 1.0 TCP）、（FASS 2.0 UDP/任务状态机）。 - /// Loop 导航：继续使用 Core LoopMission 或本插件 MagFass2LoopMission 分配 goalSite， - /// 拦截编译脚本后一次下发全程 0xB1。 + /// 车型：（FASS 1.0 TCP）、（FASS 2.0 UDP/任务状态机）。 + /// Loop 导航：继续使用 Core LoopMission 或本插件 Mag2LoopMission 分配 goalSite， + /// 拦截编译脚本后一次下发全程 0xB1。 /// 宿主（SimpleLite）加载本 dll 后反射实例化并 OnActivate / 注册。 /// public sealed class MagneticSceneProfile : NavigationProfileBase @@ -23,12 +23,12 @@ namespace StandardScene.Magnetic public override IReadOnlyList CarTypes => new[] { typeof(MagCar), - typeof(MagFass2Car), + typeof(Mag2Car), }; public override void OnActivate(ISceneContext context) { - context.Log($"{DisplayName} 已激活（车型：MagCar / MagFass2Car）"); + context.Log($"{DisplayName} 已激活（车型：MagCar / Mag2Car）"); } } } diff --git a/StandardScene.Magnetic/Properties/InternalsVisibleTo.cs b/StandardScene.Magnetic/Properties/InternalsVisibleTo.cs new file mode 100644 index 0000000..6f05413 --- /dev/null +++ b/StandardScene.Magnetic/Properties/InternalsVisibleTo.cs @@ -0,0 +1,3 @@ +using System.Runtime.CompilerServices; + +[assembly: InternalsVisibleTo("StandardScene.Magnetic.Tests")] diff --git a/StandardScene.Magnetic/Protocol/Fass2StateCodec.cs b/StandardScene.Magnetic/Protocol/Fass2StateCodec.cs index 1a827d2..bbb869c 100644 --- a/StandardScene.Magnetic/Protocol/Fass2StateCodec.cs +++ b/StandardScene.Magnetic/Protocol/Fass2StateCodec.cs @@ -9,7 +9,7 @@ namespace StandardScene.Magnetic.Protocol /// public static class Fass2StateCodec { - public static byte[] BuildState(Fass2StateReport report, string carType = "MagFass2") + public static byte[] BuildState(Fass2StateReport report, string carType = "Mag2") { if (report == null) { diff --git a/StandardScene.Magnetic/Protocol/Fass2UdpHub.cs b/StandardScene.Magnetic/Protocol/Fass2UdpHub.cs index 3c22a51..639531b 100644 --- a/StandardScene.Magnetic/Protocol/Fass2UdpHub.cs +++ b/StandardScene.Magnetic/Protocol/Fass2UdpHub.cs @@ -227,7 +227,7 @@ namespace StandardScene.Magnetic.Protocol } Diagnosis.Post( - $"Fass2UdpHub 收到 Car={carCode} 的状态，但未注册该编号（已注册: {registered}）。请检查 MagFass2Car.VehicleCode 与模拟器车号是否一致，并重新启动场景。", + $"Fass2UdpHub 收到 Car={carCode} 的状态，但未注册该编号（已注册: {registered}）。请检查 Mag2Car.VehicleCode 与模拟器车号是否一致，并重新启动场景。", "Fass2UdpHub", true); } diff --git a/StandardScene.Magnetic/StandardScene.Magnetic.scene.json b/StandardScene.Magnetic/StandardScene.Magnetic.scene.json index 1bffd27..332de2c 100644 --- a/StandardScene.Magnetic/StandardScene.Magnetic.scene.json +++ b/StandardScene.Magnetic/StandardScene.Magnetic.scene.json @@ -6,7 +6,7 @@ "coreVersion": ">=1.0.0", "requiresCore": "StandardScene.dll", "provides": { - "carTypes": [ "MagCar", "MagFass2Car" ], - "missionTypes": [ "EventCarMission", "MagFass2LoopMission" ] + "carTypes": [ "MagCar", "Mag2Car" ], + "missionTypes": [ "EventCarMission", "Mag2LoopMission" ] } } diff --git a/StandardScene.Magnetic/Tasking/Fass2ActionResolver.cs b/StandardScene.Magnetic/Tasking/Fass2ActionResolver.cs index 109bc62..a719a79 100644 --- a/StandardScene.Magnetic/Tasking/Fass2ActionResolver.cs +++ b/StandardScene.Magnetic/Tasking/Fass2ActionResolver.cs @@ -7,15 +7,15 @@ namespace StandardScene.Magnetic.Tasking { /// /// 比对期望节点与上报节点，判定到站与动作是否完成（对齐 backend CarResponseService）。 + /// 默认只闭环机构类动作；轨迹字段（Speed/Orientation 等）需显式开启。 /// public static class Fass2ActionResolver { - private static readonly (string Name, Func Get, Action Set)[] ActionFields = + /// 是否在停车站比对 Speed/Orientation/Direction/Byroad 等轨迹字段。 + public static bool VerifyTrajectoryFields { get; set; } + + private static readonly (string Name, Func Get, Action Set)[] MechanismFields = { - ("StartStop", n => n.StartStop, (n, v) => n.StartStop = v), - ("Direction", n => n.Direction, (n, v) => n.Direction = v), - ("Orientation", n => n.Orientation, (n, v) => n.Orientation = v), - ("Byroad", n => n.Byroad, (n, v) => n.Byroad = v), ("Obstacle", n => n.Obstacle, (n, v) => n.Obstacle = v), ("Audio", n => n.Audio, (n, v) => n.Audio = v), ("Light", n => n.Light, (n, v) => n.Light = v), @@ -28,6 +28,13 @@ namespace StandardScene.Magnetic.Tasking ("Shutdown", n => n.Shutdown, (n, v) => n.Shutdown = v) }; + private static readonly (string Name, Func Get, Action Set)[] TrajectoryFields = + { + ("Direction", n => n.Direction, (n, v) => n.Direction = v), + ("Orientation", n => n.Orientation, (n, v) => n.Orientation = v), + ("Byroad", n => n.Byroad, (n, v) => n.Byroad = v) + }; + public static bool IsAtNode(Fass2StateReport report, ushort targetNode) { return report?.Node != null && report.Node.Node == targetNode; @@ -35,7 +42,7 @@ namespace StandardScene.Magnetic.Tasking public static bool IsVehicleMoving(byte vehicleState) { - return vehicleState == 1; + return vehicleState is 1 or 5; } public static bool RequiresActionWait(Fass2NodeMessage expected) @@ -45,12 +52,28 @@ namespace StandardScene.Magnetic.Tasking return false; } - if (expected.StartStop is Fass2TaskBuilder.StartStopStop or Fass2TaskBuilder.StartStopPrecision) + if (expected.StartStop is Fass2TaskBuilder.StartStopStop + or Fass2TaskBuilder.StartStopPrecision + or Fass2TaskBuilder.StartStopControlStop + or Fass2TaskBuilder.StartStopControlStart) { return true; } - foreach (var field in ActionFields) + foreach (var field in MechanismFields) + { + if (field.Get(expected) != 0) + { + return true; + } + } + + if (!VerifyTrajectoryFields) + { + return false; + } + + foreach (var field in TrajectoryFields) { if (field.Get(expected) != 0) { @@ -83,12 +106,16 @@ namespace StandardScene.Magnetic.Tasking return true; } - // 过站(StartStop=1) 只要求到点且非运行态，不要求 Orientation 等轨迹字段到位。 if (expected.StartStop == Fass2TaskBuilder.StartStopPass) { return true; } + if (expected.StartStop == Fass2TaskBuilder.StartStopControlStop) + { + return false; + } + return ListPendingFields(expected, actual).Count == 0; } @@ -105,18 +132,25 @@ namespace StandardScene.Magnetic.Tasking pending.Add("StartStop"); } - if (expected.Speed != 0 && actual.Speed != expected.Speed) + if (VerifyTrajectoryFields) { - pending.Add("Speed"); - } - - foreach (var field in ActionFields) - { - if (field.Name == "StartStop") + if (expected.Speed != 0 && actual.Speed != expected.Speed) { - continue; + pending.Add("Speed"); } + foreach (var field in TrajectoryFields) + { + var expectedValue = field.Get(expected); + if (expectedValue != 0 && field.Get(actual) != expectedValue) + { + pending.Add(field.Name); + } + } + } + + foreach (var field in MechanismFields) + { var expectedValue = field.Get(expected); if (expectedValue != 0 && field.Get(actual) != expectedValue) { @@ -143,19 +177,27 @@ namespace StandardScene.Magnetic.Tasking hasPatch = true; } - if (expected.Speed != 0 && (actual == null || actual.Speed != expected.Speed)) + if (VerifyTrajectoryFields) { - patch.Speed = expected.Speed; - hasPatch = true; - } - - foreach (var field in ActionFields) - { - if (field.Name == "StartStop") + if (expected.Speed != 0 && (actual == null || actual.Speed != expected.Speed)) { - continue; + patch.Speed = expected.Speed; + hasPatch = true; } + foreach (var field in TrajectoryFields) + { + var expectedValue = field.Get(expected); + if (expectedValue != 0 && (actual == null || field.Get(actual) != expectedValue)) + { + field.Set(patch, expectedValue); + hasPatch = true; + } + } + } + + foreach (var field in MechanismFields) + { var expectedValue = field.Get(expected); if (expectedValue != 0 && (actual == null || field.Get(actual) != expectedValue)) { diff --git a/StandardScene.Magnetic/Tasking/Fass2ControlAreaGate.cs b/StandardScene.Magnetic/Tasking/Fass2ControlAreaGate.cs new file mode 100644 index 0000000..ae78667 --- /dev/null +++ b/StandardScene.Magnetic/Tasking/Fass2ControlAreaGate.cs @@ -0,0 +1,214 @@ +using StandardScene.Magnetic.Protocol; +using System; +using System.Collections.Generic; +using System.Globalization; + +namespace StandardScene.Magnetic.Tasking +{ + /// + /// 管控停止（StartStop=12）按车放行：地图站点保持 12，仅改本车任务报文。 + /// + public sealed class Fass2ControlAreaInfo + { + public int SiteId { get; set; } + public string AreaId { get; set; } = string.Empty; + public int[] AreaSiteIds { get; set; } = Array.Empty(); + public int Capacity { get; set; } = 1; + public byte ReleaseStartStop { get; set; } = Fass2TaskBuilder.StartStopControlStart; + } + + public sealed class Fass2ControlOccupancy + { + public Fass2ControlOccupancy(int carId, IReadOnlyList occupiedSiteIds) + { + CarId = carId; + OccupiedSiteIds = occupiedSiteIds ?? Array.Empty(); + } + + public int CarId { get; } + public IReadOnlyList OccupiedSiteIds { get; } + } + + public sealed class Fass2ControlReleaseCheck + { + public bool CanRelease { get; set; } + public string AreaId { get; set; } = string.Empty; + public int Capacity { get; set; } = 1; + public int OthersInArea { get; set; } + public byte ReleaseStartStop { get; set; } = Fass2TaskBuilder.StartStopControlStart; + public string Reason { get; set; } = string.Empty; + } + + public static class Fass2ControlAreaGate + { + private static readonly char[] SiteIdSeparators = { ',', ';', '|', ' ', '\t', '[', ']', '(', ')', '{', '}' }; + + public static bool IsControlStop(byte startStop) + { + return startStop == Fass2TaskBuilder.StartStopControlStop; + } + + public static bool AllowsLeave(byte startStop) + { + return startStop == Fass2TaskBuilder.StartStopPass + || startStop == Fass2TaskBuilder.StartStopControlStart; + } + + public static Fass2ControlAreaInfo Resolve(Fass2SiteActionData data, int siteId) + { + data ??= new Fass2SiteActionData(); + var areaSites = ParseSiteIds(data.ControlArea, siteId); + var capacity = data.ControlCapacity.GetValueOrDefault(1); + if (capacity < 1) + { + capacity = 1; + } + + var release = data.ControlReleaseStartStop ?? Fass2TaskBuilder.StartStopControlStart; + if (release != Fass2TaskBuilder.StartStopPass && + release != Fass2TaskBuilder.StartStopControlStart) + { + release = Fass2TaskBuilder.StartStopControlStart; + } + + return new Fass2ControlAreaInfo + { + SiteId = siteId, + AreaSiteIds = areaSites, + AreaId = string.Join(",", areaSites), + Capacity = capacity, + ReleaseStartStop = release + }; + } + + public static int[] ParseSiteIds(string text, int fallbackSiteId) + { + var ids = new List(); + if (!string.IsNullOrWhiteSpace(text)) + { + var parts = text.Split(SiteIdSeparators, StringSplitOptions.RemoveEmptyEntries); + foreach (var part in parts) + { + if (int.TryParse(part.Trim(), NumberStyles.Integer, CultureInfo.InvariantCulture, out var id) && + id > 0 && !ids.Contains(id)) + { + ids.Add(id); + } + } + } + + if (ids.Count == 0 && fallbackSiteId > 0) + { + ids.Add(fallbackSiteId); + } + + return ids.ToArray(); + } + + public static int LimitWindowCount( + IReadOnlyList nodes, + int startIndex, + int count, + int releasedIndex) + { + if (nodes == null || count <= 0 || startIndex < 0 || startIndex >= nodes.Count) + { + return 0; + } + + var limited = Math.Min(count, nodes.Count - startIndex); + for (var i = 0; i < limited; i++) + { + var node = nodes[startIndex + i]; + if (node != null && IsControlStop(node.StartStop) && releasedIndex != startIndex + i) + { + return i + 1; + } + } + + return limited; + } + + public static Fass2ControlReleaseCheck Evaluate( + int selfCarId, + Fass2ControlAreaInfo area, + IReadOnlyList occupancies) + { + area ??= new Fass2ControlAreaInfo(); + if (area.AreaSiteIds == null || area.AreaSiteIds.Length == 0) + { + area.AreaSiteIds = area.SiteId > 0 ? new[] { area.SiteId } : Array.Empty(); + } + + if (string.IsNullOrEmpty(area.AreaId)) + { + area.AreaId = string.Join(",", area.AreaSiteIds); + } + + if (area.Capacity < 1) + { + area.Capacity = 1; + } + + var others = 0; + if (occupancies != null) + { + foreach (var occupancy in occupancies) + { + if (occupancy == null || occupancy.CarId == selfCarId) + { + continue; + } + + if (OccupiesListedSites(occupancy.OccupiedSiteIds, area.AreaSiteIds)) + { + others++; + } + } + } + + var canRelease = others < area.Capacity; + return new Fass2ControlReleaseCheck + { + CanRelease = canRelease, + AreaId = area.AreaId, + Capacity = area.Capacity, + OthersInArea = others, + ReleaseStartStop = area.ReleaseStartStop == 0 + ? Fass2TaskBuilder.StartStopControlStart + : area.ReleaseStartStop, + Reason = canRelease + ? "area clear" + : $"area occupied cars={others}, capacity={area.Capacity}" + }; + } + + public static bool OccupiesListedSites(IReadOnlyList occupiedSiteIds, IReadOnlyList areaSiteIds) + { + if (occupiedSiteIds == null || occupiedSiteIds.Count == 0 || + areaSiteIds == null || areaSiteIds.Count == 0) + { + return false; + } + + for (var i = 0; i < occupiedSiteIds.Count; i++) + { + var occupied = occupiedSiteIds[i]; + if (occupied <= 0) + { + continue; + } + + for (var j = 0; j < areaSiteIds.Count; j++) + { + if (areaSiteIds[j] == occupied) + { + return true; + } + } + } + + return false; + } + } +} diff --git a/StandardScene.Magnetic/Tasking/Fass2ControlAreaOccupancy.cs b/StandardScene.Magnetic/Tasking/Fass2ControlAreaOccupancy.cs new file mode 100644 index 0000000..6563346 --- /dev/null +++ b/StandardScene.Magnetic/Tasking/Fass2ControlAreaOccupancy.cs @@ -0,0 +1,79 @@ +using SimpleCore; +using SimpleCore.PropType; +using SimpleLite.RCS.CarTypes; +using System; +using System.Collections.Generic; +using System.Linq; + +namespace StandardScene.Magnetic.Tasking +{ + /// + /// 按 holdingLocks + 物理 siteID 统计管控站点列表上的他车占用。 + /// + internal static class Fass2ControlAreaOccupancy + { + public static Fass2ControlReleaseCheck Check(Car self, int controlSiteId) + { + var site = SimpleLib.GetSite(controlSiteId); + var area = Fass2ControlAreaGate.Resolve(Fass2SiteFieldReader.Read(site), controlSiteId); + var occupancies = new List(); + + IEnumerable cars; + try + { + cars = SimpleLib.GetAllCars(); + } + catch (Exception ex) + { + return new Fass2ControlReleaseCheck + { + CanRelease = false, + AreaId = area.AreaId, + Capacity = area.Capacity, + ReleaseStartStop = area.ReleaseStartStop, + Reason = "car list unavailable: " + ex.Message + }; + } + + if (cars != null) + { + foreach (var other in cars.OfType()) + { + if (other == null) + { + continue; + } + + occupancies.Add(new Fass2ControlOccupancy(other.id, CollectOccupiedSiteIds(other))); + } + } + + return Fass2ControlAreaGate.Evaluate(self?.id ?? 0, area, occupancies); + } + + private static IReadOnlyList CollectOccupiedSiteIds(Car car) + { + var ids = new List(); + if (car.siteID > 0) + { + ids.Add(car.siteID); + } + + var holding = car.status?.holdingLocks; + if (holding == null) + { + return ids; + } + + foreach (var lockId in holding) + { + if (lockId > 0 && !ids.Contains(lockId)) + { + ids.Add(lockId); + } + } + + return ids; + } + } +} diff --git a/StandardScene.Magnetic/Tasking/Fass2PathFinder.cs b/StandardScene.Magnetic/Tasking/Fass2PathFinder.cs index 88719a6..71fe599 100644 --- a/StandardScene.Magnetic/Tasking/Fass2PathFinder.cs +++ b/StandardScene.Magnetic/Tasking/Fass2PathFinder.cs @@ -5,7 +5,7 @@ namespace StandardScene.Magnetic.Tasking { internal static class Fass2PathFinder { - public static List GetSitesBetween(int startSiteId, int endSiteId) + public static List GetSitesBetweenBfs(int startSiteId, int endSiteId) { if (startSiteId == endSiteId) { diff --git a/StandardScene.Magnetic/Tasking/Fass2ReconnectChainSelector.cs b/StandardScene.Magnetic/Tasking/Fass2ReconnectChainSelector.cs new file mode 100644 index 0000000..f12eba8 --- /dev/null +++ b/StandardScene.Magnetic/Tasking/Fass2ReconnectChainSelector.cs @@ -0,0 +1,127 @@ +using System.Collections.Generic; +using System.Linq; + +namespace StandardScene.Magnetic.Tasking +{ + /// + /// 断线重连后，用当前占点匹配 tasklist 链路的结果。 + /// + public sealed class Fass2ChainMatch + { + public int TaskId { get; set; } + public int TaskPriority { get; set; } + public int CurrentSiteId { get; set; } + public int TargetSiteId { get; set; } + public int IndexInPath { get; set; } + public int DistanceToTarget { get; set; } + public bool IsAtStartPoint { get; set; } + public bool IsAtEndPoint { get; set; } + public IReadOnlyList FullPath { get; set; } = new List(); + public IReadOnlyList RemainingPath { get; set; } = new List(); + + public override string ToString() + { + var position = IsAtStartPoint ? "start" : (IsAtEndPoint ? "end" : "mid"); + return $"task={TaskId} {CurrentSiteId}->{TargetSiteId} pos={position} remain={DistanceToTarget} prio={TaskPriority}"; + } + } + + /// + /// 一站多链路时的固定裁决：原 goal 仍在剩余路径 → 起点 → 优先级 → 距目标更近。 + /// + public static class Fass2ReconnectChainSelector + { + public static bool TrySelect( + IReadOnlyList matches, + int? preferGoalSiteId, + out Fass2ChainMatch selected, + out string reason) + { + selected = null; + reason = "no chain"; + if (matches == null || matches.Count == 0) + { + return false; + } + + var candidates = matches.Where(m => m != null && m.TargetSiteId > 0).ToList(); + if (candidates.Count == 0) + { + return false; + } + + if (preferGoalSiteId != null && preferGoalSiteId.Value > 0) + { + var keepGoal = candidates + .Where(m => RemainingContains(m, preferGoalSiteId.Value)) + .ToList(); + if (keepGoal.Count == 1) + { + selected = keepGoal[0]; + reason = $"preferGoal={preferGoalSiteId.Value}"; + return true; + } + + if (keepGoal.Count > 1) + { + candidates = keepGoal; + reason = $"preferGoal={preferGoalSiteId.Value} narrowed={keepGoal.Count}"; + } + } + + var ranked = candidates + .OrderByDescending(m => m.IsAtStartPoint ? 1 : 0) + .ThenByDescending(m => m.TaskPriority) + .ThenBy(m => m.DistanceToTarget) + .ThenBy(m => m.TaskId) + .ToList(); + + if (ranked.Count == 0) + { + return false; + } + + var best = ranked[0]; + if (ranked.Count > 1) + { + var second = ranked[1]; + var tied = best.IsAtStartPoint == second.IsAtStartPoint && + best.TaskPriority == second.TaskPriority && + best.DistanceToTarget == second.DistanceToTarget; + if (tied && best.TargetSiteId != second.TargetSiteId) + { + reason = $"ambiguous {best} vs {second}"; + return false; + } + } + + selected = best; + if (string.IsNullOrEmpty(reason) || reason == "no chain") + { + reason = $"selected {best}"; + } + else + { + reason = $"{reason}; selected {best}"; + } + + return true; + } + + private static bool RemainingContains(Fass2ChainMatch match, int siteId) + { + if (match.RemainingPath != null) + { + for (var i = 0; i < match.RemainingPath.Count; i++) + { + if (match.RemainingPath[i] == siteId) + { + return true; + } + } + } + + return match.TargetSiteId == siteId; + } + } +} diff --git a/StandardScene.Magnetic/Tasking/Fass2RouteHelper.cs b/StandardScene.Magnetic/Tasking/Fass2RouteHelper.cs new file mode 100644 index 0000000..21fa9f9 --- /dev/null +++ b/StandardScene.Magnetic/Tasking/Fass2RouteHelper.cs @@ -0,0 +1,210 @@ +using SimpleCore; +using SimpleCore.Compiler; +using SimpleCore.Library; +using SimpleCore.PropType; +using System; +using System.Collections.Generic; + +namespace StandardScene.Magnetic.Tasking +{ + /// + /// 从站点序列构建 ，供交管 Forecast 与任务下发共用同一路径。 + /// + internal static class Fass2RouteHelper + { + public static List GetSitesBetween(int startSiteId, int endSiteId, AbstractCar car) + { + if (startSiteId == endSiteId) + { + return new List { startSiteId }; + } + + if (car != null) + { + var startSite = SimpleLib.GetSite(startSiteId); + var endSite = SimpleLib.GetSite(endSiteId); + if (startSite != null && endSite != null) + { + var plan = new SegmentPlan { usingCar = car }; + plan.fields["allow_destination_on_route"] = "true"; + plan.FindRoute(startSite, endSite); + var sites = ExtractSiteIds(plan); + if (sites.Count > 0) + { + return sites; + } + } + } + + return Fass2PathFinder.GetSitesBetweenBfs(startSiteId, endSiteId); + } + + public static void ForecastTrafficSequence(AbstractCar car, IReadOnlyList siteIds, int fromIndex) + { + if (car == null || siteIds == null || siteIds.Count == 0) + { + throw new ArgumentException("traffic forecast requires car and siteIds"); + } + + fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1)); + if (fromIndex >= siteIds.Count - 1) + { + return; + } + + var routeSiteIds = new List(); + for (var i = fromIndex; i < siteIds.Count; i++) + { + routeSiteIds.Add(siteIds[i]); + } + + try + { + var plan = BuildSegmentPlan(car, routeSiteIds); + var program = plan.Compile("fass2-traffic", false); + program.Forecast(); + } + catch (Exception ex) when (IsRecoverableForecastFailure(ex)) + { + // 双车 Multi-car search Exhausted 时降级：只建 pending/seqScope，交由 TryLock 现场占点 + ApplyManualTrafficSequence(car, routeSiteIds); + Diagnosis.Post( + $"Mag2Car forecast fallback manual sequence car={car.id}: {ex.Message}", + "Mag2Car", + true); + } + } + + /// + /// 不走 SimpleCore 多车 Forecast，直接写入交管序列（持锁起点已由 TrafficReset 保证）。 + /// + public static void ApplyManualTrafficSequence(AbstractCar car, IReadOnlyList routeSiteIds) + { + if (car == null || routeSiteIds == null || routeSiteIds.Count == 0) + { + return; + } + + var scope = new int[routeSiteIds.Count]; + for (var i = 0; i < routeSiteIds.Count; i++) + { + scope[i] = routeSiteIds[i]; + } + + var pending = new int[Math.Max(0, scope.Length - 1)]; + for (var i = 0; i < pending.Length; i++) + { + pending[i] = scope[i + 1]; + } + + car.status.seqScope = scope; + car.status.pendingLocks = pending; + car.status.seqPtr = scope.Length > 1 ? 1 : 0; + car.status.escape = Array.Empty(); + } + + private static bool IsRecoverableForecastFailure(Exception ex) + { + for (var cur = ex; cur != null; cur = cur.InnerException) + { + var msg = cur.Message ?? string.Empty; + if (msg.IndexOf("Multi-car traffic search", StringComparison.OrdinalIgnoreCase) >= 0 || + msg.IndexOf("Exhausted", StringComparison.OrdinalIgnoreCase) >= 0 || + msg.IndexOf("BadForecast", StringComparison.OrdinalIgnoreCase) >= 0 || + cur.GetType().Name.IndexOf("Forecast", StringComparison.OrdinalIgnoreCase) >= 0) + { + return true; + } + } + + return false; + } + + public static SegmentPlan BuildSegmentPlan(AbstractCar car, IReadOnlyList siteIds) + { + if (car == null || siteIds == null || siteIds.Count == 0) + { + throw new ArgumentException("segment plan requires car and siteIds"); + } + + var plan = new SegmentPlan { usingCar = car }; + plan.fields["allow_destination_on_route"] = "true"; + + var firstSite = SimpleLib.GetSite(siteIds[0]); + if (firstSite == null) + { + throw new InvalidOperationException($"site {siteIds[0]} not found"); + } + + plan.segments.Add(firstSite); + + for (var i = 0; i < siteIds.Count - 1; i++) + { + var dstSite = SimpleLib.GetSite(siteIds[i + 1]); + if (dstSite == null) + { + throw new InvalidOperationException($"site {siteIds[i + 1]} not found"); + } + + var track = FindTrack(siteIds[i], siteIds[i + 1]); + if (track == null) + { + throw new InvalidOperationException($"no track from site {siteIds[i]} to {siteIds[i + 1]}"); + } + + plan.segments.Add(track); + plan.segments.Add(dstSite); + } + + return plan; + } + + public static List ExtractSiteIds(SegmentPlan plan) + { + var sites = new List(); + if (plan?.segments == null) + { + return sites; + } + + foreach (var segment in plan.segments) + { + if (segment is Site site) + { + sites.Add(site.id); + } + } + + return sites; + } + + public static Track FindTrack(int fromSiteId, int toSiteId) + { + foreach (var track in SimpleLib.GetAllTracks()) + { + if (track.direction == 0) + { + if ((track.siteA == fromSiteId && track.siteB == toSiteId) || + (track.siteB == fromSiteId && track.siteA == toSiteId)) + { + return track; + } + + continue; + } + + if (track.direction == 1 && track.siteA == fromSiteId && track.siteB == toSiteId) + { + return track; + } + + if (track.direction == 2 && track.siteB == fromSiteId && track.siteA == toSiteId) + { + return track; + } + } + + return null; + } + } +} diff --git a/StandardScene.Magnetic/Tasking/Fass2SiteFieldReader.cs b/StandardScene.Magnetic/Tasking/Fass2SiteFieldReader.cs index dd3bca8..14176f1 100644 --- a/StandardScene.Magnetic/Tasking/Fass2SiteFieldReader.cs +++ b/StandardScene.Magnetic/Tasking/Fass2SiteFieldReader.cs @@ -31,6 +31,15 @@ namespace StandardScene.Magnetic.Tasking data.Shutdown = TryReadByte(site.fields, Fass2SiteFields.Shutdown); data.PrecisionStop = TryReadBool(site.fields, Fass2SiteFields.PrecisionStop); data.WaitMode = TryReadString(site.fields, Fass2SiteFields.WaitMode); + data.ControlArea = TryReadString(site.fields, Fass2SiteFields.ControlArea); + data.ControlCapacity = TryReadInt(site.fields, Fass2SiteFields.ControlCapacity); + data.ControlReleaseStartStop = TryReadByte(site.fields, Fass2SiteFields.ControlReleaseStartStop); + + if (!data.ControlCapacity.HasValue) + { + TryFillRegionCapacityFallback(site.fields, data); + } + return data; } @@ -99,5 +108,40 @@ namespace StandardScene.Magnetic.Tasking { return fields.TryGetValue(key, out var text) ? text : null; } + + private static int? TryReadInt(Dictionary fields, string key) + { + if (!fields.TryGetValue(key, out var text) || string.IsNullOrWhiteSpace(text)) + { + return null; + } + + if (int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out var value)) + { + return value; + } + + return null; + } + + private static void TryFillRegionCapacityFallback(Dictionary fields, Fass2SiteActionData data) + { + foreach (var pair in fields) + { + if (string.IsNullOrEmpty(pair.Key) || + !pair.Key.StartsWith("Region", StringComparison.OrdinalIgnoreCase)) + { + continue; + } + + if (int.TryParse(pair.Value, NumberStyles.Integer, CultureInfo.InvariantCulture, out var maxCount) && + maxCount > 0) + { + data.ControlCapacity = maxCount; + } + + return; + } + } } } diff --git a/StandardScene.Magnetic/Tasking/Fass2SiteFields.cs b/StandardScene.Magnetic/Tasking/Fass2SiteFields.cs index 64ac340..debeffe 100644 --- a/StandardScene.Magnetic/Tasking/Fass2SiteFields.cs +++ b/StandardScene.Magnetic/Tasking/Fass2SiteFields.cs @@ -23,6 +23,9 @@ namespace StandardScene.Magnetic.Tasking public const string Shutdown = "Fass2_Shutdown"; public const string PrecisionStop = "Fass2_PrecisionStop"; public const string WaitMode = "Fass2_WaitMode"; + public const string ControlArea = "Fass2_ControlArea"; + public const string ControlCapacity = "Fass2_ControlCapacity"; + public const string ControlReleaseStartStop = "Fass2_ControlReleaseStartStop"; } /// @@ -46,5 +49,8 @@ namespace StandardScene.Magnetic.Tasking public byte? Shutdown { get; set; } public bool PrecisionStop { get; set; } public string WaitMode { get; set; } + public string ControlArea { get; set; } + public int? ControlCapacity { get; set; } + public byte? ControlReleaseStartStop { get; set; } } } diff --git a/StandardScene.Magnetic/Tasking/Fass2TaskBuilder.cs b/StandardScene.Magnetic/Tasking/Fass2TaskBuilder.cs index 20e4344..d39a2a5 100644 --- a/StandardScene.Magnetic/Tasking/Fass2TaskBuilder.cs +++ b/StandardScene.Magnetic/Tasking/Fass2TaskBuilder.cs @@ -3,22 +3,23 @@ using SimpleCore.PropType; using StandardScene.Magnetic.Protocol; using System; using System.Collections.Generic; -using System.Linq; namespace StandardScene.Magnetic.Tasking { /// /// 将站点路径与站点/边 fields 合并为 FASS 2.0 0xB1 节点序列。 - /// 对齐 backend CarRequestService.GetSendNodes 的组包规则。 + /// 对齐 backend CarRequestService.GetSendNodes：边属性挂在节点出边上。 /// public static class Fass2TaskBuilder { public const byte StartStopPass = 1; public const byte StartStopStop = 2; + public const byte StartStopControlStart = 11; + public const byte StartStopControlStop = 12; public const byte StartStopPrecision = 22; public static Fass2TaskPlan BuildPath(int startSiteId, int goalSiteId, Fass2TaskBuildOptions options, - Func resolveNodeId) + Func resolveNodeId, AbstractCar routingCar = null) { if (resolveNodeId == null) { @@ -26,7 +27,7 @@ namespace StandardScene.Magnetic.Tasking } options ??= new Fass2TaskBuildOptions(); - var siteIds = Fass2PathFinder.GetSitesBetween(startSiteId, goalSiteId); + var siteIds = Fass2RouteHelper.GetSitesBetween(startSiteId, goalSiteId, routingCar); if (siteIds.Count == 0) { throw new InvalidOperationException($"no path from site {startSiteId} to {goalSiteId}"); @@ -45,9 +46,17 @@ namespace StandardScene.Magnetic.Tasking } public static Fass2NodeMessage[] BuildSegment(int srcSiteId, int dstSiteId, Fass2TaskBuildOptions options, - Func resolveNodeId) + Func resolveNodeId, AbstractCar routingCar = null) { - return BuildPath(srcSiteId, dstSiteId, options, resolveNodeId).Nodes.ToArray(); + var plan = BuildPath(srcSiteId, dstSiteId, options, resolveNodeId, routingCar); + var nodes = plan.Nodes; + var array = new Fass2NodeMessage[nodes.Count]; + for (var i = 0; i < nodes.Count; i++) + { + array[i] = nodes[i]; + } + + return array; } public static IReadOnlyList SplitBatches(IReadOnlyList nodes, int maxPerFrame = 10) @@ -88,14 +97,15 @@ namespace StandardScene.Magnetic.Tasking var site = SimpleLib.GetSite(siteId); var siteAction = Fass2SiteFieldReader.Read(site); var isLast = i == siteIds.Count - 1; - var prevSiteId = i > 0 ? siteIds[i - 1] : siteId; + var nextSiteId = isLast ? -1 : siteIds[i + 1]; Fass2TrackMotionData trackMotion = null; - Site prevSite = null; - if (i > 0) + Site currentSite = site; + Site nextSite = null; + if (!isLast) { - prevSite = SimpleLib.GetSite(prevSiteId); - var track = FindTrack(prevSiteId, siteId); + nextSite = SimpleLib.GetSite(nextSiteId); + var track = Fass2RouteHelper.FindTrack(siteId, nextSiteId); trackMotion = Fass2TrackFieldReader.Read(track); } @@ -103,9 +113,11 @@ namespace StandardScene.Magnetic.Tasking { Node = resolveNodeId(siteId), StartStop = ResolveStartStop(siteAction, isLast), - Distance = i > 0 ? ComputeEdgeDistance(prevSite, site) : (ushort)0, - Speed = SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed, options.CarSpeed), - Orientation = ResolveOrientation(prevSite, site, trackMotion), + Distance = !isLast ? ComputeEdgeDistance(currentSite, nextSite) : (ushort)0, + Speed = !isLast + ? SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed) + : (ushort)0, + Orientation = !isLast ? ResolveOrientation(currentSite, nextSite, trackMotion) : (byte)0, Byroad = siteAction.Byroad ?? trackMotion?.Byroad ?? 0, Direction = siteAction.Direction ?? trackMotion?.Direction ?? 0, Lift = siteAction.Lift ?? 0, @@ -185,42 +197,17 @@ namespace StandardScene.Magnetic.Tasking return (ushort)Math.Min(ushort.MaxValue, Math.Max(1, Math.Round(length))); } - private static ushort SpeedToProtocol(double trackSpeed, double carSpeed) + /// + /// 协议速度单位 0.1 m/min；地图 Speed 默认 m/min，≤1 视为旧版 m/s 兼容。 + /// + public static ushort SpeedToProtocol(double speed) { - var value = trackSpeed <= 0 ? carSpeed : trackSpeed; - var protocolSpeed = value <= 1 - ? (int)Math.Round(value * 600) - : (int)Math.Round(value * 10); - return (ushort)Math.Max(1, Math.Min(10000, protocolSpeed)); - } - - private static Track FindTrack(int fromSiteId, int toSiteId) - { - foreach (var track in SimpleLib.GetAllTracks()) + if (speed > 0 && speed <= 1) { - if (track.direction == 0) - { - if ((track.siteA == fromSiteId && track.siteB == toSiteId) || - (track.siteB == fromSiteId && track.siteA == toSiteId)) - { - return track; - } - - continue; - } - - if (track.direction == 1 && track.siteA == fromSiteId && track.siteB == toSiteId) - { - return track; - } - - if (track.direction == 2 && track.siteB == fromSiteId && track.siteA == toSiteId) - { - return track; - } + speed *= 60; } - return null; + return (ushort)Math.Max(1, Math.Min(10000, Math.Round(speed * 10))); } } } diff --git a/StandardScene.Magnetic/Tasking/Fass2TaskContext.cs b/StandardScene.Magnetic/Tasking/Fass2TaskContext.cs index 2e69241..cfb3513 100644 --- a/StandardScene.Magnetic/Tasking/Fass2TaskContext.cs +++ b/StandardScene.Magnetic/Tasking/Fass2TaskContext.cs @@ -36,6 +36,18 @@ namespace StandardScene.Magnetic.Tasking public DateTime LastDispatchAt { get; set; } = DateTime.MinValue; public ulong LastActionId { get; set; } public DateTime LastActionSentAt { get; set; } = DateTime.MinValue; + + /// 交管窗口未齐、正在等锁；此期间不消耗移动超时预算。 + public bool WaitingForTraffic { get; set; } + + /// 停在管控停止点等待按车放行；此期间不消耗移动超时预算。 + public bool WaitingForRelease { get; set; } + + /// 本任务已对哪个路径下标发过放行报文；-1 表示尚未放行。 + public int ControlReleasedIndex { get; set; } = -1; + + /// 最近一次规划的站点链路，供断线重连判断是否仍在原路径上。 + public int[] RouteSiteIds { get; set; } = Array.Empty(); } public static class Fass2TaskPersistence @@ -47,6 +59,7 @@ namespace StandardScene.Magnetic.Tasking public const string TagTaskId = "Fass2Task_TaskId"; public const string TagFieldsSignature = "Fass2Task_FieldsSig"; public const string TagDefaultSpeed = "Fass2Task_DefaultSpeed"; + public const string TagRouteSites = "Fass2Task_RouteSites"; public static void Save(TagSet tags, Fass2TaskContext context) { @@ -62,6 +75,7 @@ namespace StandardScene.Magnetic.Tasking SetTag(tags, TagTaskId, context.TaskId.ToString()); SetTag(tags, TagFieldsSignature, context.FieldsSignature ?? string.Empty); SetTag(tags, TagDefaultSpeed, context.DefaultSpeed.ToString(System.Globalization.CultureInfo.InvariantCulture)); + SetTag(tags, TagRouteSites, FormatIntList(context.RouteSiteIds)); } public static bool TryLoad(TagSet tags, out Fass2TaskContext context) @@ -93,6 +107,7 @@ namespace StandardScene.Magnetic.Tasking System.Globalization.CultureInfo.InvariantCulture, out var defaultSpeed); + tags.TryGetValue(TagRouteSites, out var routeSitesText); context = new Fass2TaskContext { Phase = phase, @@ -101,7 +116,8 @@ namespace StandardScene.Magnetic.Tasking CurrentIndex = currentIndex, TaskId = taskId, FieldsSignature = fieldsSignature ?? string.Empty, - DefaultSpeed = defaultSpeed + DefaultSpeed = defaultSpeed, + RouteSiteIds = ParseIntList(routeSitesText) }; return true; } @@ -120,6 +136,7 @@ namespace StandardScene.Magnetic.Tasking RemoveTag(tags, TagTaskId); RemoveTag(tags, TagFieldsSignature); RemoveTag(tags, TagDefaultSpeed); + RemoveTag(tags, TagRouteSites); } private static void SetTag(TagSet tags, string key, string value) @@ -145,5 +162,30 @@ namespace StandardScene.Magnetic.Tasking value = 0; return tags.TryGetValue(key, out var text) && int.TryParse(text, out value); } + + private static string FormatIntList(int[] values) + { + return values == null || values.Length == 0 ? string.Empty : string.Join(",", values); + } + + private static int[] ParseIntList(string text) + { + if (string.IsNullOrWhiteSpace(text)) + { + return Array.Empty(); + } + + var parts = text.Split(new[] { ',' }, StringSplitOptions.RemoveEmptyEntries); + var ids = new List(parts.Length); + foreach (var part in parts) + { + if (int.TryParse(part.Trim(), out var id) && id > 0) + { + ids.Add(id); + } + } + + return ids.ToArray(); + } } } diff --git a/StandardScene.Magnetic/Tasking/Fass2TaskPlan.cs b/StandardScene.Magnetic/Tasking/Fass2TaskPlan.cs index c91d723..ada091b 100644 --- a/StandardScene.Magnetic/Tasking/Fass2TaskPlan.cs +++ b/StandardScene.Magnetic/Tasking/Fass2TaskPlan.cs @@ -6,7 +6,7 @@ namespace StandardScene.Magnetic.Tasking public sealed class Fass2TaskBuildOptions { public bool UseTagValueAsNode { get; set; } - public double DefaultSpeed { get; set; } = 0.2; + public double DefaultSpeed { get; set; } = 12; public double CarSpeed { get; set; } = 1; public int MaxNodesPerFrame { get; set; } = 10; } diff --git a/StandardScene.Magnetic/Tasking/Fass2TaskStateMachine.cs b/StandardScene.Magnetic/Tasking/Fass2TaskStateMachine.cs index 91b0f0c..1079a49 100644 --- a/StandardScene.Magnetic/Tasking/Fass2TaskStateMachine.cs +++ b/StandardScene.Magnetic/Tasking/Fass2TaskStateMachine.cs @@ -3,6 +3,7 @@ using SimpleCore.PropType; using StandardScene.Magnetic.Protocol; using System; using System.Collections.Generic; +using System.Linq; using System.Threading; using System.Threading.Tasks; @@ -21,6 +22,27 @@ namespace StandardScene.Magnetic.Tasking public Action ClearPersisted { get; set; } public Func AllocateTaskId { get; set; } public Func AllocateActionId { get; set; } + + /// 规划完成后 Forecast 建序（seqScope/pendingLocks）。 + public Action, int> PrepareTraffic { get; set; } + + /// 返回当前可下发窗口长度（已锁站点数，含当前站）。 + public Func, int, int, int> EnsureTrafficWindow { get; set; } + + /// 段完成后释放刚离开的站点。 + public Action LeaveTraffic { get; set; } + + /// 按路径索引释放后方锁点（keepFromIndex 之前）。 + public Action, int> ReleaseTrafficBehind { get; set; } + + /// 重连对齐：Reset 当前站并从 fromIndex 重建前方窗口。 + public Action, int> RebaseTraffic { get; set; } + + /// 全程完成，保留终点 holding。 + public Action FinalizeTraffic { get; set; } + + /// 管控停止到站后，判断同区他车占用是否已允许放行本车。 + public Func CheckControlRelease { get; set; } } public sealed class Fass2TaskTickResult @@ -110,6 +132,10 @@ namespace StandardScene.Magnetic.Tasking Context.LastDispatchAt = DateTime.MinValue; Context.LastActionSentAt = DateTime.MinValue; Context.LastActionId = 0; + Context.WaitingForTraffic = false; + Context.WaitingForRelease = false; + Context.ControlReleasedIndex = -1; + Context.RouteSiteIds = Array.Empty(); Log($"task begin start={startSiteId}, goal={goalSiteId}, speed={defaultSpeed}"); Persist(); } @@ -142,6 +168,10 @@ namespace StandardScene.Magnetic.Tasking Context.LastActionSentAt = DateTime.MinValue; Context.Plan = null; Context.FaultReason = null; + Context.RouteSiteIds = saved.RouteSiteIds ?? Array.Empty(); + Context.WaitingForTraffic = false; + Context.WaitingForRelease = false; + Context.ControlReleasedIndex = -1; if (Context.Phase == Fass2TaskPhase.Planning) { @@ -157,6 +187,134 @@ namespace StandardScene.Magnetic.Tasking } } + public bool TryFindIndexOnRoute(int siteId, out int index) + { + lock (_syncRoot) + { + return TryFindIndexOnRouteUnlocked(siteId, out index); + } + } + + public bool IsAtRouteEnd(int siteId) + { + lock (_syncRoot) + { + var sites = GetRouteSiteIds(); + return sites.Count > 0 && sites[sites.Count - 1] == siteId; + } + } + + /// + /// 上报站仍在原任务链路上：对齐 index，从该站重建后方锁窗口，继续同一 goal。 + /// 不取消等待中的任务。 + /// + public bool RebaseToSite(int siteId) + { + lock (_syncRoot) + { + if (!IsRunning) + { + return false; + } + + if (!TryEnsurePlanUnlocked()) + { + return false; + } + + if (!TryFindIndexOnRouteUnlocked(siteId, out var index)) + { + return false; + } + + Context.CurrentIndex = index; + RememberRoute(Context.Plan); + try + { + if (_callbacks.RebaseTraffic != null) + { + _callbacks.RebaseTraffic(Context.Plan.SiteIds, index); + } + else + { + _callbacks.PrepareTraffic?.Invoke(Context.Plan.SiteIds, index); + } + } + catch (Exception ex) + { + Log($"reconnect rebase traffic wait: {ex.Message}"); + Context.WaitingForTraffic = true; + Context.Phase = Fass2TaskPhase.Moving; + Context.LastDispatchAt = DateTime.MinValue; + Context.StartedAt = DateTime.Now; + Persist(); + return true; + } + + Context.WaitingForTraffic = false; + Context.WaitingForRelease = false; + Context.Phase = Fass2TaskPhase.Moving; + Context.LastDispatchAt = DateTime.MinValue; + Context.StartedAt = DateTime.Now; + Log($"reconnect rebase site={siteId}, index={index}, goal={Context.GoalSiteId}"); + Persist(); + return true; + } + } + + /// + /// 不在原链路：保留等待句柄，从新站重开同一/新 goal。 + /// + public bool RestartFromSite(int startSiteId, int goalSiteId) + { + lock (_syncRoot) + { + if (!IsRunning) + { + return false; + } + + Context.Phase = Fass2TaskPhase.Planning; + Context.StartSiteId = startSiteId; + Context.GoalSiteId = goalSiteId; + Context.CurrentIndex = 0; + Context.Plan = null; + Context.RouteSiteIds = Array.Empty(); + Context.TaskId = 0; + Context.FieldsSignature = string.Empty; + Context.LastDispatchAt = DateTime.MinValue; + Context.LastActionSentAt = DateTime.MinValue; + Context.LastActionId = 0; + Context.StartedAt = DateTime.Now; + Context.WaitingForTraffic = false; + Context.WaitingForRelease = false; + Context.ControlReleasedIndex = -1; + Context.FaultReason = null; + Log($"reconnect restart start={startSiteId}, goal={goalSiteId}"); + Persist(); + return true; + } + } + + public bool TryCompleteAtSite(int siteId) + { + lock (_syncRoot) + { + if (IsIdle) + { + return false; + } + + Context.Phase = Fass2TaskPhase.Complete; + TryFinalizeTraffic(); + ClearPersisted(); + Log($"reconnect complete at site={siteId}"); + Completed?.Invoke(Context); + _completionSource?.TrySetResult(1); + return true; + } + } + public Fass2TaskTickResult Tick(Fass2StateReport report) { lock (_syncRoot) @@ -216,6 +374,7 @@ namespace StandardScene.Magnetic.Tasking Context.Phase = Fass2TaskPhase.Cancelled; Context.FaultReason = reason; Log($"task cancelled: {reason}"); + TryFinalizeTraffic(); ClearPersisted(); _completionSource?.TrySetException(new OperationCanceledException(reason)); } @@ -235,7 +394,6 @@ namespace StandardScene.Magnetic.Tasking waiter = _completionSource ?? ResetCompletionSource(); } - var timeoutTask = Task.Delay(timeoutMs, cancellationToken); var pollTask = Task.Run(async () => { while (!cancellationToken.IsCancellationRequested) @@ -264,19 +422,12 @@ namespace StandardScene.Magnetic.Tasking } }, cancellationToken); - var finished = await Task.WhenAny(waiter.Task, timeoutTask); - if (finished == timeoutTask) - { - Cancel("wait timeout"); - throw new TimeoutException( - $"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}"); - } - - await waiter.Task; + await WaitWithTrafficPauseAsync(waiter, timeoutMs, cancellationToken); } /// /// UDP 模式由 Hub 回调驱动 Tick，此处只等待完成信号，避免与 OnUdpStateReceived 争用锁导致死锁。 + /// 等锁（WaitingForTraffic）期间不消耗 timeoutMs。 /// public async Task WaitForCompletionAsync(int timeoutMs, CancellationToken cancellationToken = default) { @@ -291,13 +442,85 @@ namespace StandardScene.Magnetic.Tasking waiter = _completionSource ?? ResetCompletionSource(); } - var timeoutTask = Task.Delay(timeoutMs, cancellationToken); - var finished = await Task.WhenAny(waiter.Task, timeoutTask); - if (finished == timeoutTask) + await WaitWithTrafficPauseAsync(waiter, timeoutMs, cancellationToken); + } + + /// + /// 移动超时倒计时；交管等锁或管控等待放行时暂停扣减。 + /// + private async Task WaitWithTrafficPauseAsync(TaskCompletionSource waiter, int timeoutMs, + CancellationToken cancellationToken) + { + if (waiter == null) { - Cancel("wait timeout"); - throw new TimeoutException( - $"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}"); + throw new ArgumentNullException(nameof(waiter)); + } + + var remainingMs = timeoutMs <= 0 ? -1 : timeoutMs; + const int sliceMs = 500; + var lastPauseLog = DateTime.MinValue; + + while (true) + { + cancellationToken.ThrowIfCancellationRequested(); + + if (waiter.Task.IsCompleted) + { + await waiter.Task; + return; + } + + var delayMs = remainingMs < 0 ? sliceMs : Math.Min(sliceMs, Math.Max(1, remainingMs)); + var delayTask = Task.Delay(delayMs, cancellationToken); + var finished = await Task.WhenAny(waiter.Task, delayTask); + if (finished == waiter.Task) + { + await waiter.Task; + return; + } + + bool waitingPaused; + string pauseKind; + Fass2TaskPhase phase; + int index; + int goal; + lock (_syncRoot) + { + phase = Context.Phase; + index = Context.CurrentIndex; + goal = Context.GoalSiteId; + waitingPaused = Context.WaitingForTraffic || Context.WaitingForRelease; + pauseKind = Context.WaitingForRelease ? "control wait" : "traffic wait"; + + if (phase is Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault) + { + break; + } + } + + if (waitingPaused) + { + if ((DateTime.Now - lastPauseLog).TotalSeconds >= 5) + { + lastPauseLog = DateTime.Now; + Log($"timeout paused ({pauseKind}), remain={(remainingMs < 0 ? "inf" : remainingMs + "ms")}, index={index}, goal={goal}"); + } + + continue; + } + + if (remainingMs < 0) + { + continue; + } + + remainingMs -= delayMs; + if (remainingMs <= 0) + { + Cancel("wait timeout"); + throw new TimeoutException( + $"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}"); + } } await waiter.Task; @@ -319,13 +542,33 @@ namespace StandardScene.Magnetic.Tasking Context.FieldsSignature = Context.Plan.FieldsSignature; Context.TaskId = _callbacks.AllocateTaskId(); + Context.ControlReleasedIndex = -1; + Context.WaitingForRelease = false; + RememberRoute(Context.Plan); + + try + { + _callbacks.PrepareTraffic?.Invoke(Context.Plan.SiteIds, Context.CurrentIndex); + } + catch (Exception ex) + { + return Fault($"traffic prepare failed: {ex.Message}", result); + } if (StartBeforeMove && Context.CurrentIndex == 0) { - _callbacks.SendControl(Fass2Protocol.CmdStart, HeadingAngle); + _callbacks.SendControl(Fass2Protocol.CmdStart, 0); + } + + if (!TryDispatchCurrentWindow(out var waitReason)) + { + result.Message = waitReason; + Context.Phase = Fass2TaskPhase.Moving; + result.Phase = Context.Phase; + Persist(); + return result; } - DispatchCurrentWindow(); Context.Phase = Fass2TaskPhase.Moving; result.Phase = Context.Phase; result.Dispatched = true; @@ -337,17 +580,27 @@ namespace StandardScene.Magnetic.Tasking private Fass2TaskTickResult HandleMoving(Fass2StateReport report, Fass2TaskTickResult result) { - if (TryRebuildForFieldsChange(result)) + if (TryRebuildForFieldsChange(report, result)) { return result; } if (ShouldResend()) { - DispatchCurrentWindow(); - result.Dispatched = true; + if (TryDispatchCurrentWindow(out var waitReason)) + { + result.Dispatched = true; + } + else if (string.IsNullOrEmpty(result.Message)) + { + result.Message = waitReason; + } } + // 滑动窗口一次下发多站时，过站点可能被车体直接飞过；上报已到更后站则追赶 Leave + 推进 index。 + TryCatchUpPassedSites(report, result); + ReleaseLocksBehindVehicle(report); + var expected = GetExpectedNode(); if (!Fass2ActionResolver.IsAtNode(report, expected.Node)) { @@ -357,7 +610,11 @@ namespace StandardScene.Magnetic.Tasking if (Fass2ActionResolver.IsVehicleMoving(report.State)) { - result.Message = $"at node={expected.Node}, still running"; + // 仍报到本站时不要 Leave/推进：模拟器过站常保持 node=当前站并增加 dist。 + // 真正飞过到更后站时由 TryCatchUpPassedSites 推进并放锁。 + result.Message = Fass2ActionResolver.RequiresActionWait(expected) + ? $"at node={expected.Node}, still running" + : $"pass node={expected.Node}, still running (wait leave)"; return result; } @@ -371,43 +628,55 @@ namespace StandardScene.Magnetic.Tasking private Fass2TaskTickResult HandleAtStation(Fass2StateReport report, Fass2TaskTickResult result) { - if (TryRebuildForFieldsChange(result)) + if (TryRebuildForFieldsChange(report, result)) { return result; } if (ShouldResend()) { - DispatchCurrentWindow(); - result.Dispatched = true; + if (TryDispatchCurrentWindow(out var waitReason)) + { + result.Dispatched = true; + } + else if (string.IsNullOrEmpty(result.Message)) + { + result.Message = waitReason; + } } + if (TryCatchUpPassedSites(report, result)) + { + Context.WaitingForRelease = false; + ReleaseLocksBehindVehicle(report); + Context.Phase = Fass2TaskPhase.Moving; + result.Phase = Context.Phase; + result.Message = $"catch-up from AtStation to index={Context.CurrentIndex}, node={report.Node.Node}"; + return result; + } + + ReleaseLocksBehindVehicle(report); + var expected = GetExpectedNode(); if (!Fass2ActionResolver.IsAtNode(report, expected.Node)) { + Context.WaitingForRelease = false; Context.Phase = Fass2TaskPhase.Moving; result.Phase = Context.Phase; result.Message = $"left station node={report.Node.Node}, expect={expected.Node}"; return result; } + if (TryHandleControlStop(report, expected, result)) + { + return result; + } + + Context.WaitingForRelease = false; + if (!Fass2ActionResolver.IsStationActionComplete(expected, report.Node, report.State)) { - if (ShouldSendActionPatch()) - { - var patch = Fass2ActionResolver.BuildActionPatch(expected, report.Node); - if (patch != null) - { - Context.LastActionId = _callbacks.AllocateActionId(); - Context.LastActionSentAt = DateTime.Now; - _callbacks.SendAction(patch, Context.LastActionId); - result.ActionSent = true; - result.Message = - $"action pending [{Fass2ActionResolver.DescribePending(expected, report.Node)}], sent 0xA1={Context.LastActionId}"; - Log(result.Message); - } - } - + TrySendActionPatch(expected, report, result); return result; } @@ -419,13 +688,157 @@ namespace StandardScene.Magnetic.Tasking return result; } + private bool TryHandleControlStop(Fass2StateReport report, Fass2NodeMessage expected, Fass2TaskTickResult result) + { + var unreleased = Fass2ControlAreaGate.IsControlStop(expected.StartStop) + && Context.ControlReleasedIndex != Context.CurrentIndex; + var releasedHere = Context.ControlReleasedIndex == Context.CurrentIndex; + if (!unreleased && !releasedHere) + { + return false; + } + + Context.WaitingForRelease = true; + var isLast = Context.Plan != null && Context.CurrentIndex >= Context.Plan.Nodes.Count - 1; + + if (Fass2ActionResolver.IsVehicleMoving(report.State)) + { + Context.Phase = Fass2TaskPhase.Moving; + result.Phase = Context.Phase; + result.Message = $"control node={expected.Node}, vehicle running"; + return true; + } + + if (unreleased) + { + if (report.Node.StartStop != Fass2TaskBuilder.StartStopControlStop) + { + TrySendActionPatch(expected, report, result); + result.Message = $"control stop pending StartStop, node={expected.Node}"; + return true; + } + + var siteId = ResolveControlSiteId(); + var check = _callbacks.CheckControlRelease?.Invoke(siteId) + ?? new Fass2ControlReleaseCheck + { + CanRelease = true, + ReleaseStartStop = Fass2TaskBuilder.StartStopControlStart, + Reason = "no occupancy callback" + }; + + if (!check.CanRelease) + { + result.Message = + $"control wait site={siteId}, area={check.AreaId}, others={check.OthersInArea}/{check.Capacity}, {check.Reason}"; + return true; + } + + var releaseSs = check.ReleaseStartStop == 0 + ? Fass2TaskBuilder.StartStopControlStart + : check.ReleaseStartStop; + expected.StartStop = releaseSs; + Context.ControlReleasedIndex = Context.CurrentIndex; + SendControlReleasePatch(expected, releaseSs, result); + if (TryDispatchCurrentWindow(out var waitReason)) + { + result.Dispatched = true; + } + else if (string.IsNullOrEmpty(result.Message)) + { + result.Message = waitReason; + } + + result.Message = + $"control released site={siteId}, 12->{releaseSs}, area={check.AreaId}, others={check.OthersInArea}/{check.Capacity}"; + Log(result.Message); + Persist(); + return true; + } + + if (report.Node.StartStop != expected.StartStop) + { + TrySendActionPatch(expected, report, result); + result.Message = $"control release pending StartStop={expected.StartStop}, node={expected.Node}"; + return true; + } + + if (isLast) + { + Context.WaitingForRelease = false; + Context.Phase = Fass2TaskPhase.SegmentDone; + result.Phase = Context.Phase; + result.Message = $"control last-station done index={Context.CurrentIndex}, node={expected.Node}"; + Log(result.Message); + Persist(); + return true; + } + + result.Message = $"control released waiting leave, node={expected.Node}"; + return true; + } + + private void TrySendActionPatch(Fass2NodeMessage expected, Fass2StateReport report, Fass2TaskTickResult result) + { + if (!ShouldSendActionPatch()) + { + return; + } + + var patch = Fass2ActionResolver.BuildActionPatch(expected, report?.Node); + if (patch == null) + { + return; + } + + Context.LastActionId = _callbacks.AllocateActionId(); + Context.LastActionSentAt = DateTime.Now; + _callbacks.SendAction(patch, Context.LastActionId); + result.ActionSent = true; + result.Message = + $"action pending [{Fass2ActionResolver.DescribePending(expected, report?.Node)}], sent 0xA1={Context.LastActionId}"; + Log(result.Message); + } + + private void SendControlReleasePatch(Fass2NodeMessage expected, byte releaseStartStop, Fass2TaskTickResult result) + { + var patch = new Fass2NodeMessage + { + Node = expected.Node, + StartStop = releaseStartStop + }; + Context.LastActionId = _callbacks.AllocateActionId(); + Context.LastActionSentAt = DateTime.Now; + _callbacks.SendAction(patch, Context.LastActionId); + result.ActionSent = true; + } + + private int ResolveControlSiteId() + { + if (Context.Plan?.SiteIds != null && + Context.CurrentIndex >= 0 && + Context.CurrentIndex < Context.Plan.SiteIds.Count) + { + return Context.Plan.SiteIds[Context.CurrentIndex]; + } + + return Context.StartSiteId; + } + private Fass2TaskTickResult HandleSegmentDone(Fass2StateReport report, Fass2TaskTickResult result) { + var finishedIndex = Context.CurrentIndex; Context.CurrentIndex++; result.Advanced = true; + Context.WaitingForRelease = false; if (Context.Plan == null || Context.CurrentIndex >= Context.Plan.Nodes.Count) { + if (Context.Plan?.SiteIds != null && finishedIndex < Context.Plan.SiteIds.Count) + { + _callbacks.FinalizeTraffic?.Invoke(Context.Plan.SiteIds[finishedIndex]); + } + Context.Phase = Fass2TaskPhase.Complete; result.Phase = Context.Phase; result.Completed = true; @@ -437,8 +850,23 @@ namespace StandardScene.Magnetic.Tasking return result; } + if (Context.Plan.SiteIds != null && finishedIndex < Context.Plan.SiteIds.Count) + { + _callbacks.LeaveTraffic?.Invoke(Context.Plan.SiteIds[finishedIndex]); + } + + ReleaseLocksBehindVehicle(report); + Context.Phase = Fass2TaskPhase.Dispatching; - DispatchCurrentWindow(); + if (!TryDispatchCurrentWindow(out var waitReason)) + { + Context.Phase = Fass2TaskPhase.Moving; + result.Phase = Context.Phase; + result.Message = waitReason; + Persist(); + return result; + } + Context.Phase = Fass2TaskPhase.Moving; result.Phase = Context.Phase; result.Dispatched = true; @@ -448,7 +876,7 @@ namespace StandardScene.Magnetic.Tasking return result; } - private bool TryRebuildForFieldsChange(Fass2TaskTickResult result) + private bool TryRebuildForFieldsChange(Fass2StateReport report, Fass2TaskTickResult result) { if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0) { @@ -462,7 +890,8 @@ namespace StandardScene.Magnetic.Tasking return false; } - var currentSiteId = Context.Plan.SiteIds[Context.CurrentIndex]; + // 优先用车辆实际报到站重建，避免逻辑 index 超前时 Reset 到他车正占用的前方站。 + var currentSiteId = ResolveRebuildStartSiteId(report); Log($"fields changed at index={Context.CurrentIndex}, rebuild from site={currentSiteId}"); Context.Plan = _callbacks.BuildPlan(currentSiteId, Context.GoalSiteId, Context.DefaultSpeed); if (Context.Plan == null || Context.Plan.Nodes.Count == 0) @@ -474,7 +903,35 @@ namespace StandardScene.Magnetic.Tasking Context.CurrentIndex = 0; Context.FieldsSignature = Context.Plan.FieldsSignature; Context.TaskId = _callbacks.AllocateTaskId(); - DispatchCurrentWindow(); + Context.ControlReleasedIndex = -1; + Context.WaitingForRelease = false; + RememberRoute(Context.Plan); + try + { + _callbacks.PrepareTraffic?.Invoke(Context.Plan.SiteIds, Context.CurrentIndex); + } + catch (Exception ex) + { + // 起点被他车占用等：等待，不要把整单打成 Fault + Context.Phase = Fass2TaskPhase.Moving; + result.Phase = Context.Phase; + result.Rebuilt = true; + result.Message = $"traffic rebuild prepare wait: {ex.Message}"; + Log(result.Message); + Persist(); + return true; + } + + if (!TryDispatchCurrentWindow(out var waitReason)) + { + Context.Phase = Fass2TaskPhase.Moving; + result.Phase = Context.Phase; + result.Rebuilt = true; + result.Message = waitReason; + Persist(); + return true; + } + Context.Phase = Fass2TaskPhase.Moving; result.Phase = Context.Phase; result.Rebuilt = true; @@ -484,28 +941,73 @@ namespace StandardScene.Magnetic.Tasking return true; } - private void DispatchCurrentWindow() + private int ResolveRebuildStartSiteId(Fass2StateReport report) { - var window = BuildDispatchWindow(); + if (report?.Node != null && _callbacks.ResolveSite != null) + { + var physical = _callbacks.ResolveSite(report.Node.Node); + if (physical != null && physical.id > 0) + { + return physical.id; + } + } + + if (Context.Plan?.SiteIds != null && + Context.CurrentIndex >= 0 && + Context.CurrentIndex < Context.Plan.SiteIds.Count) + { + return Context.Plan.SiteIds[Context.CurrentIndex]; + } + + return Context.StartSiteId; + } + + private bool TryDispatchCurrentWindow(out string waitReason) + { + waitReason = null; + var window = BuildDispatchWindow(out var lockedCount, out var wantWindow); if (window.Length == 0) { - throw new InvalidOperationException("dispatch window is empty"); + waitReason = "dispatch window is empty"; + Context.WaitingForTraffic = true; + return false; + } + + if (_callbacks.EnsureTrafficWindow != null && lockedCount < wantWindow && lockedCount <= 1) + { + waitReason = $"traffic wait, locked={lockedCount}/{wantWindow}, index={Context.CurrentIndex}"; + Context.WaitingForTraffic = true; + return false; } _callbacks.SendNodes(window, Context.TaskId); Context.LastDispatchAt = DateTime.Now; - Log($"dispatch window count={window.Length}, fromIndex={Context.CurrentIndex}, task={Context.TaskId}"); + Context.WaitingForTraffic = false; + Log($"dispatch window count={window.Length}, locked={lockedCount}/{wantWindow}, fromIndex={Context.CurrentIndex}, task={Context.TaskId}"); + return true; } - private Fass2NodeMessage[] BuildDispatchWindow() + private Fass2NodeMessage[] BuildDispatchWindow(out int lockedCount, out int wantWindow) { var nodes = Context.Plan.Nodes; var start = Context.CurrentIndex; var remaining = nodes.Count - start; - var windowSize = Math.Max(1, Math.Min(LockCount, remaining)); - windowSize = Math.Min(windowSize, 10); - var window = new Fass2NodeMessage[windowSize]; - for (var i = 0; i < windowSize; i++) + wantWindow = Math.Max(1, Math.Min(LockCount, remaining)); + wantWindow = Math.Min(wantWindow, 10); + wantWindow = Fass2ControlAreaGate.LimitWindowCount( + nodes, start, wantWindow, Context.ControlReleasedIndex); + + lockedCount = wantWindow; + if (_callbacks.EnsureTrafficWindow != null && Context.Plan?.SiteIds != null) + { + lockedCount = _callbacks.EnsureTrafficWindow(Context.Plan.SiteIds, start, wantWindow); + } + + lockedCount = Fass2ControlAreaGate.LimitWindowCount( + nodes, start, lockedCount, Context.ControlReleasedIndex); + lockedCount = Math.Max(1, Math.Min(lockedCount, wantWindow)); + var window = new Fass2NodeMessage[lockedCount]; + for (var i = 0; i < lockedCount; i++) { window[i] = nodes[start + i]; } @@ -518,6 +1020,99 @@ namespace StandardScene.Magnetic.Tasking return Context.Plan.Nodes[Context.CurrentIndex]; } + /// + /// 当上报节点已是计划中更后方站点时，释放中间站交管锁并推进 CurrentIndex。 + /// + private bool TryCatchUpPassedSites(Fass2StateReport report, Fass2TaskTickResult result) + { + if (Context.Plan?.Nodes == null || report?.Node == null) + { + return false; + } + + var reportIndex = FindPlanIndexByNode(report.Node.Node, Context.CurrentIndex); + if (reportIndex <= Context.CurrentIndex) + { + return false; + } + + var catchTo = reportIndex; + for (var i = Context.CurrentIndex; i < reportIndex; i++) + { + if (Fass2ControlAreaGate.IsControlStop(Context.Plan.Nodes[i].StartStop) && + Context.ControlReleasedIndex != i) + { + catchTo = i; + break; + } + } + + if (catchTo <= Context.CurrentIndex) + { + return false; + } + + while (Context.CurrentIndex < catchTo) + { + var finishedIndex = Context.CurrentIndex; + if (Context.Plan.SiteIds != null && finishedIndex < Context.Plan.SiteIds.Count) + { + _callbacks.LeaveTraffic?.Invoke(Context.Plan.SiteIds[finishedIndex]); + } + + Context.CurrentIndex++; + result.Advanced = true; + Log( + $"catch-up passed index={finishedIndex} -> {Context.CurrentIndex}, reportNode={report.Node.Node}"); + } + + Persist(); + return true; + } + + /// + /// 以车辆实际报到站为准释放身后锁；逻辑 index 若超前于物理站，不得放掉车辆仍在的站。 + /// + private void ReleaseLocksBehindVehicle(Fass2StateReport report) + { + if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0) + { + return; + } + + var keepIndex = Context.CurrentIndex; + if (report?.Node != null) + { + var reportIndex = FindPlanIndexByNode(report.Node.Node, 0); + if (reportIndex >= 0) + { + // 取更靠后的“尚未离开”位置，避免 CurrentIndex 超前时把物理站 Leave 掉 + keepIndex = Math.Min(keepIndex, reportIndex); + } + } + + _callbacks.ReleaseTrafficBehind?.Invoke(Context.Plan.SiteIds, keepIndex); + } + + private int FindPlanIndexByNode(ushort node, int fromIndex) + { + if (Context.Plan?.Nodes == null) + { + return -1; + } + + fromIndex = Math.Max(0, fromIndex); + for (var i = fromIndex; i < Context.Plan.Nodes.Count; i++) + { + if (Context.Plan.Nodes[i].Node == node) + { + return i; + } + } + + return -1; + } + private int ResolveCurrentSiteId(Fass2StateReport report) { if (Context.CurrentIndex > 0 && Context.Plan?.SiteIds != null && @@ -528,7 +1123,7 @@ namespace StandardScene.Magnetic.Tasking if (report?.Node != null && report.Node.Node != 0) { - var site = _callbacks.ResolveSite(report.Node.Node); + var site = _callbacks.ResolveSite?.Invoke(report.Node.Node); if (site != null) { return site.id; @@ -558,12 +1153,115 @@ namespace StandardScene.Magnetic.Tasking result.Faulted = true; result.Message = reason; Log($"task fault: {reason}"); + TryFinalizeTraffic(); Persist(); Faulted?.Invoke(Context, reason); _completionSource?.TrySetException(new InvalidOperationException(reason)); return result; } + private void TryFinalizeTraffic() + { + if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0) + { + return; + } + + var index = Math.Max(0, Math.Min(Context.CurrentIndex, Context.Plan.SiteIds.Count - 1)); + _callbacks.FinalizeTraffic?.Invoke(Context.Plan.SiteIds[index]); + } + + private void RememberRoute(Fass2TaskPlan plan) + { + if (plan?.SiteIds == null || plan.SiteIds.Count == 0) + { + Context.RouteSiteIds = Array.Empty(); + return; + } + + var copy = new int[plan.SiteIds.Count]; + for (var i = 0; i < plan.SiteIds.Count; i++) + { + copy[i] = plan.SiteIds[i]; + } + + Context.RouteSiteIds = copy; + } + + private IReadOnlyList GetRouteSiteIds() + { + if (Context.Plan?.SiteIds != null && Context.Plan.SiteIds.Count > 0) + { + return Context.Plan.SiteIds; + } + + return Context.RouteSiteIds ?? Array.Empty(); + } + + private bool TryFindIndexOnRouteUnlocked(int siteId, out int index) + { + index = -1; + var sites = GetRouteSiteIds(); + if (sites == null || sites.Count == 0 || siteId <= 0) + { + return false; + } + + var from = Math.Max(0, Math.Min(Context.CurrentIndex, sites.Count - 1)); + for (var i = from; i < sites.Count; i++) + { + if (sites[i] == siteId) + { + index = i; + return true; + } + } + + for (var i = from - 1; i >= 0; i--) + { + if (sites[i] == siteId) + { + index = i; + return true; + } + } + + return false; + } + + private bool TryEnsurePlanUnlocked() + { + if (Context.Plan?.Nodes != null && Context.Plan.Nodes.Count > 0) + { + RememberRoute(Context.Plan); + return true; + } + + if (_callbacks.BuildPlan == null || Context.GoalSiteId <= 0) + { + return false; + } + + var startId = Context.StartSiteId > 0 ? Context.StartSiteId : Context.GoalSiteId; + try + { + Context.Plan = _callbacks.BuildPlan(startId, Context.GoalSiteId, Context.DefaultSpeed); + } + catch (Exception ex) + { + Log($"reconnect rebuild plan failed: {ex.Message}"); + return false; + } + + if (Context.Plan?.Nodes == null || Context.Plan.Nodes.Count == 0) + { + return false; + } + + RememberRoute(Context.Plan); + return true; + } + private void Persist() { _callbacks.Persist?.Invoke(Context); diff --git a/StandardScene.Magnetic/Tasking/Fass2TrafficLocker.cs b/StandardScene.Magnetic/Tasking/Fass2TrafficLocker.cs new file mode 100644 index 0000000..bcd3e04 --- /dev/null +++ b/StandardScene.Magnetic/Tasking/Fass2TrafficLocker.cs @@ -0,0 +1,303 @@ +using SimpleCore; +using SimpleCore.Compiler; +using SimpleCore.PropType; +using SimpleCore.Traffic; +using System; +using System.Collections.Generic; +using System.Linq; + +namespace StandardScene.Magnetic.Tasking +{ + /// + /// 方案 B′：用任务下发的 siteIds 经 SegmentPlan.Forecast 建立 seqScope/pendingLocks，再段级 TryLock/Leave。 + /// + internal sealed class Fass2TrafficLocker + { + private readonly AbstractCar _car; + private readonly Action _log; + + public Fass2TrafficLocker(AbstractCar car, Action log) + { + _car = car ?? throw new ArgumentNullException(nameof(car)); + _log = log ?? (_ => { }); + } + + public void PrepareSequence(IReadOnlyList siteIds, int fromIndex) + { + if (siteIds == null || siteIds.Count == 0) + { + throw new InvalidOperationException("traffic plan siteIds is empty"); + } + + fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1)); + var startSiteId = siteIds[fromIndex]; + var startSite = SimpleLib.GetSite(startSiteId); + if (startSite == null) + { + throw new InvalidOperationException($"traffic start site invalid: {startSiteId}"); + } + + // TrafficReset 会 programs.Clear() 把 now 置空；TryLock 在真正占点前要求 programs.now != null， + // 否则抛 Program obsoleted。环线拦截路径仍处在 move 脚本的 actualSendScript 中，需保留/重建锚点。 + var keepProgram = _car.status.programs.now; + + _log($"prepare reset start={startSiteId}, goal={siteIds[siteIds.Count - 1]}, fromIndex={fromIndex}, route=[{string.Join(",", siteIds)}]"); + try + { + _car.TrafficReset(startSite, makeAvailable: true, strict: false); + } + catch (Exception ex) when (IsStartOccupiedByOtherCar(ex)) + { + // 起点被他车占用时不能 Reset；若本车已持有该站则跳过 Reset 继续建序，否则交给上层等待。 + if (Array.IndexOf(_car.status.holdingLocks, startSiteId) < 0) + { + throw new InvalidOperationException( + $"traffic start site {startSiteId} occupied by another car", ex); + } + + _log($"prepare reset skipped, already holding start={startSiteId}, ex={ex.Message}"); + } + + EnsureProgramAnchor(keepProgram); + + if (fromIndex >= siteIds.Count - 1) + { + LogTrafficState("prepare single-site"); + return; + } + + Fass2RouteHelper.ForecastTrafficSequence(_car, siteIds, fromIndex); + LogTrafficState("prepare forecast ok"); + } + + public void RebaseFrom(IReadOnlyList siteIds, int fromIndex, int windowSize) + { + PrepareSequence(siteIds, fromIndex); + EnsureWindowLocked(siteIds, fromIndex, Math.Max(1, windowSize)); + } + + private static bool IsStartOccupiedByOtherCar(Exception ex) + { + for (var cur = ex; cur != null; cur = cur.InnerException) + { + var msg = cur.Message ?? string.Empty; + if (msg.IndexOf("already locks it", StringComparison.OrdinalIgnoreCase) >= 0 || + msg.IndexOf("unavailable site", StringComparison.OrdinalIgnoreCase) >= 0) + { + return true; + } + } + + return false; + } + + public bool TryLockNext(int siteId) + { + if (!_car.status.usage.Get().scheduling) + { + throw new InvalidOperationException("abandoned"); + } + + if (Array.IndexOf(_car.status.holdingLocks, siteId) >= 0) + { + return true; + } + + if (_car.status.pendingLocks.Length > 0 && _car.status.pendingLocks[0] != siteId) + { + // 窗口重入时可能再次请求已锁过的更早站点；交给 EnsureWindowLocked 跳过，避免 UDP 线程抛异常。 + _log($"lock skip site={siteId}, expect pending0={_car.status.pendingLocks[0]}, holding=[{FormatInts(_car.status.holdingLocks)}]"); + return false; + } + + EnsureProgramAnchor(keepProgram: null); + + if (TrafficControl.TryLock(_car, siteId)) + { + _log($"lock ok site={siteId}, holding=[{FormatInts(_car.status.holdingLocks)}]"); + return true; + } + + _log($"lock wait site={siteId}, blockedBy={FormatBlocked()}, TCStat={FormatTcStat()}"); + return false; + } + + public void LeavePassed(int siteId) + { + if (_car.status.holdingLocks.Length <= 1) + { + _log($"leave skip site={siteId}, holding count={_car.status.holdingLocks.Length}"); + return; + } + + if (Array.IndexOf(_car.status.holdingLocks, siteId) < 0) + { + _log($"leave skip site={siteId}, not in holding"); + return; + } + + EnsureProgramAnchor(keepProgram: null); + TrafficControl.Leave(_car, siteId); + _log($"leave site={siteId}, holding=[{FormatInts(_car.status.holdingLocks)}]"); + } + + /// + /// 释放路径上严格位于 keepFromIndex 之前的持锁站点（当前站及前方窗口保留）。 + /// + public void ReleaseLocksBehind(IReadOnlyList siteIds, int keepFromIndex) + { + if (siteIds == null || siteIds.Count == 0 || _car.status.holdingLocks.Length <= 1) + { + return; + } + + keepFromIndex = Math.Max(0, Math.Min(keepFromIndex, siteIds.Count - 1)); + var holdingSnapshot = _car.status.holdingLocks.ToArray(); + foreach (var siteId in holdingSnapshot) + { + if (_car.status.holdingLocks.Length <= 1) + { + break; + } + + var index = IndexOfSite(siteIds, siteId); + if (index < 0 || index >= keepFromIndex) + { + continue; + } + + LeavePassed(siteId); + } + } + + private static int IndexOfSite(IReadOnlyList siteIds, int siteId) + { + for (var i = 0; i < siteIds.Count; i++) + { + if (siteIds[i] == siteId) + { + return i; + } + } + + return -1; + } + + public int EnsureWindowLocked(IReadOnlyList siteIds, int fromIndex, int windowSize) + { + if (siteIds == null || siteIds.Count == 0) + { + return 0; + } + + fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1)); + windowSize = Math.Max(1, Math.Min(windowSize, 10)); + var maxIndex = Math.Min(siteIds.Count - 1, fromIndex + windowSize - 1); + + // 返回值必须是从 fromIndex 起的连续已占/新锁站数，供 0xB1 窗口切片使用。 + // UDP Tick 会反复进入：已在 holding 的站直接计数，禁止再次对已消费的 pending 站 TryLock。 + var lockedCount = 0; + for (var i = fromIndex; i <= maxIndex; i++) + { + var siteId = siteIds[i]; + if (Array.IndexOf(_car.status.holdingLocks, siteId) >= 0) + { + lockedCount++; + continue; + } + + if (_car.status.pendingLocks.Length == 0 || _car.status.pendingLocks[0] != siteId) + { + _log( + $"window gap site={siteId}, pending0={(_car.status.pendingLocks.Length > 0 ? _car.status.pendingLocks[0].ToString() : "-")}, holding=[{FormatInts(_car.status.holdingLocks)}]"); + break; + } + + if (!TryLockNext(siteId)) + { + break; + } + + lockedCount++; + } + + if (lockedCount == 0) + { + lockedCount = 1; + _log($"window warn: no holding overlap index={fromIndex}, site={siteIds[fromIndex]}, holding=[{FormatInts(_car.status.holdingLocks)}]"); + } + + _log($"window locked={lockedCount}/{windowSize}, fromIndex={fromIndex}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}]"); + return lockedCount; + } + + public void FinalizeAtSite(int siteId) + { + _log($"finalize site={siteId}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}]"); + } + + /// + /// SimpleCore.TryLock 在写锁前检查 programs.now；为空则抛 Program obsoleted。 + /// + private void EnsureProgramAnchor(CarProgram keepProgram) + { + if (_car.status.programs.now != null) + { + return; + } + + if (keepProgram != null) + { + _car.status.programs.now = keepProgram; + _log($"restore programs.now after TrafficReset, name={keepProgram.name}, state={keepProgram.status.state}"); + return; + } + + var dummy = new CarProgram + { + name = $"fass2-traffic-anchor:{_car.id}", + plans = new[] + { + new SegmentPlan { usingCar = _car } + } + }; + dummy.status.state = CarProgram.StatusEnum.Dummy; + _car.status.programs.now = dummy; + _log($"install dummy programs.now for TryLock, name={dummy.name}"); + } + + private void LogTrafficState(string prefix) + { + _log( + $"{prefix}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}], seqScope=[{FormatInts(_car.status.seqScope)}], seqPtr={_car.status.seqPtr}"); + } + + private string FormatBlocked() + { + if (_car.status.blockedBy == null || _car.status.blockedBy.Length == 0) + { + return "-"; + } + + return string.Join(",", _car.status.blockedBy.Select(p => $"{p.Item1}:{p.Item2}")); + } + + private string FormatTcStat() + { + try + { + var tc = _car.status.TCStat; + return string.IsNullOrWhiteSpace(tc) ? "-" : tc; + } + catch + { + return "-"; + } + } + + private static string FormatInts(int[] values) + { + return values == null || values.Length == 0 ? string.Empty : string.Join(",", values); + } + } +} diff --git a/StandardScene.Magnetic/Tasking/MagFass2CarFileLogger.cs b/StandardScene.Magnetic/Tasking/Mag2CarFileLogger.cs similarity index 89% rename from StandardScene.Magnetic/Tasking/MagFass2CarFileLogger.cs rename to StandardScene.Magnetic/Tasking/Mag2CarFileLogger.cs index 2fbe2d3..24c920c 100644 --- a/StandardScene.Magnetic/Tasking/MagFass2CarFileLogger.cs +++ b/StandardScene.Magnetic/Tasking/Mag2CarFileLogger.cs @@ -6,9 +6,9 @@ using System.Text; namespace StandardScene.Magnetic.Tasking { /// - /// MagFass2Car 本地文件日志，按协议车号分目录：logs/car{VehicleCode}/magfass2_yyyyMMdd.log + /// Mag2Car 本地文件日志，按协议车号分目录：logs/car{VehicleCode}/mag2_yyyyMMdd.log /// - public static class MagFass2CarFileLogger + public static class Mag2CarFileLogger { private static readonly ConcurrentDictionary CarLocks = new ConcurrentDictionary(); private static string _baseDirectory = "logs"; @@ -27,7 +27,7 @@ namespace StandardScene.Magnetic.Tasking public static string GetCurrentLogFilePath(ushort vehicleCode) { - var fileName = $"magfass2_{DateTime.Now:yyyyMMdd}.log"; + var fileName = $"mag2_{DateTime.Now:yyyyMMdd}.log"; return Path.Combine(GetCarLogDirectory(vehicleCode), fileName); } diff --git a/StandardScene.Magnetic/docs/MagFass2Car使用指南.html b/StandardScene.Magnetic/docs/Mag2Car使用指南.html similarity index 95% rename from StandardScene.Magnetic/docs/MagFass2Car使用指南.html rename to StandardScene.Magnetic/docs/Mag2Car使用指南.html index a9142d6..87254b4 100644 --- a/StandardScene.Magnetic/docs/MagFass2Car使用指南.html +++ b/StandardScene.Magnetic/docs/Mag2Car使用指南.html @@ -2,7 +2,7 @@ -MagFass2Car Guide +Mag2Car Guide -

MagFass2Car（FASS 2.0）使用指南

+

Mag2Car（FASS 2.0）使用指南

适用对象：第一次接触磁导航 + FASS 2.0 的调试/实施人员

插件：`StandardScene.Magnetic.dll`

-

车型：`MagFass2Car`

+

车型：`Mag2Car`

最后更新：2026-07

目录

@@ -57,7 +57,7 @@

1. 这套系统能做什么

-

MagFass2Car 是 StandardScene **磁导航平台插件**里的 FASS 2.0 车型，主要能力：

+

Mag2Car 是 StandardScene **磁导航平台插件**里的 FASS 2.0 车型，主要能力：

@@ -96,12 +96,12 @@

步骤 1：加载插件

确认场景已加载 scene.mag（StandardScene.Magnetic.scene.json），提供：

    -
  • 车型：MagCar、MagFass2Car
    • -
  • 进程：EventCarMission、MagFass2LoopMission
    • +
  • 车型：MagCar、Mag2Car
    • +
  • 进程：EventCarMission、Mag2LoopMission

步骤 2：添加一辆车

    -
  1. 在场景中添加车型 **MagFass2Car**
    2. +
  2. 在场景中添加车型 **Mag2Car**
  3. 填写基本参数（UDP 模式示例）：
能力
@@ -150,8 +150,8 @@

**方式 A — 手动 Go（Web/调度下发）**

用系统自带的「去某站」功能，内部会编译脚本，例如：


-agv.MagFass2Go(1, 2, 0.2);
-agv.MagFass2Go(2, 5, 0.2);
+agv.Mag2Go(1, 2, 0.2);
+agv.Mag2Go(2, 5, 0.2);

**方式 B — LoopMission 环线**

    @@ -165,11 +165,11 @@ agv.MagFass2Go(2, 5, 0.2); 
    
 ┌─────────────────────────────────────────────────────────┐
 │  业务层：LoopMission / 手动 Go / 脚本                    │
-│  （分配 goalSite 或编译多行 MagFass2Go）                  │
+│  （分配 goalSite 或编译多行 Mag2Go）                  │
 └────────────────────────┬────────────────────────────────┘
                          ▼
 ┌─────────────────────────────────────────────────────────┐
-│  MagFass2Car 执行层                                      │
+│  Mag2Car 执行层                                      │
 │  · 脚本拦截（Loop + goalSite）→ 一次全程任务              │
 │  · Fass2TaskBuilder：路径 + fields → 节点序列             │
 │  · Fass2TaskStateMachine：滑动窗口下发 + 动作闭环         │
@@ -186,7 +186,7 @@ agv.MagFass2Go(2, 5, 0.2);
 
    **关键理解：**
    
 
      
 
    • **LoopMission（Core）** 只负责「给谁分配哪个目标站」（写 goalSite 标签）
      • 
-
    • **MagFass2Car** 负责「怎么走过去」（拼包、下发、等到站）
      • 
+
    • **Mag2Car** 负责「怎么走过去」（拼包、下发、等到站）
      • 
 
    • **阶段 4** 后，Loop 不再逐行执行多段脚本，而是**拦截为一次全程任务**（可关）
      • 
 
    
 
    
@@ -195,8 +195,8 @@ agv.MagFass2Go(2, 5, 0.2);
 
    文件：StandardScene.Magnetic.scene.json
    
 
    
 {
-  "carTypes": [ "MagCar", "MagFass2Car" ],
-  "missionTypes": [ "EventCarMission", "MagFass2LoopMission" ]
+  "carTypes": [ "MagCar", "Mag2Car" ],
+  "missionTypes": [ "EventCarMission", "Mag2LoopMission" ]
 }
 
    
 
    4.2 选哪个 Mission？
    
@@ -210,21 +210,21 @@ agv.MagFass2Go(2, 5, 0.2);
- + - + - +
LoopMission Core通用环线，MagFass2Car 同样可用通用环线，Mag2Car 同样可用
磁导航FASS2环线（MagFass2LoopMission）磁导航FASS2环线（Mag2LoopMission） Magnetic 插件 逻辑与 LoopMission 相同，名称标识磁导航 FASS2 场景
FASS车辆事件进程（EventCarMission） Magnetic 插件**仅 MagCar（FASS 1.0）**，不驱动 MagFass2Car**仅 MagCar（FASS 1.0）**，不驱动 Mag2Car
-

新手建议：MagFass2Car 环线用 **`LoopMission`** 或 **`磁导航FASS2环线`** 均可。

+

新手建议：Mag2Car 环线用 **`LoopMission`** 或 **`磁导航FASS2环线`** 均可。

4.3 tasklist.json 最小示例


 {
@@ -253,7 +253,7 @@ agv.MagFass2Go(2, 5, 0.2);
 
含义：车到站 1 → 自动分配目标站 2；到站 2 → 自动分配目标站 1，形成环线。

 

 
5. 车辆参数说明

-
在场景编辑器中选中 MagFass2Car，常用字段如下。

+
在场景编辑器中选中 Mag2Car，常用字段如下。

 
5.1 通讯

 
@@ -426,8 +426,8 @@ agv.MagFass2Go(2, 5, 0.2);
 
**流程**：

 FindRoute → Compile → 多行脚本：
-  agv.MagFass2Go(1,2,speed);
-  agv.MagFass2Go(2,3,speed);
+  agv.Mag2Go(1,2,speed);
+  agv.Mag2Go(2,3,speed);
   ...
 → SelfEvaluating 逐行执行
 → 每行：锁终点 → 发 0xB1 → 等到站 → 释放起点
@@ -435,7 +435,7 @@ FindRoute → Compile → 多行脚本：
 
**特点**：每**条边**一段任务，多次 taskId，与交管「逐段锁点」一致。

 

 
方式 2：Loop 全程任务（阶段 4，默认）

-
**触发**：goalSite 标签 + 编译脚本含 MagFass2Go + EnableLoopTaskDrive=true。

+
**触发**：goalSite 标签 + 编译脚本含 Mag2Go + EnableLoopTaskDrive=true。

 
**流程**：

 

 LoopMission 写 goalSite
@@ -446,7 +446,7 @@ LoopMission 写 goalSite
 

 
**特点**：业务上一次任务，协议上仍遵守每帧 ≤10 站。

 

-
方式 3：单段 MagFass2Go（脚本一行）

+
方式 3：单段 Mag2Go（脚本一行）

 
**触发**：脚本里只有一行，或无 goalSite 的测试。

 
**流程**：与方式 1 的单行相同，状态机只跑 src → dst 一段。

 

@@ -631,7 +631,7 @@ LoopMission 写 goalSite
 

-
+
@@ -1295,7 +1295,7 @@ LoopMission 写 goalSite
 
-
+
@@ -1433,7 +1433,7 @@ Fass2_Obstacle = 2
 2. 匹配 tasklist：currentStationId = N → 分配 targetStationId
 3. 写 tags：goalSite = 目标站 ID
 4. LoopStartAction 调用 GoSite
-5. MagFass2Car 拦截 → 全程任务到 goalSite
+5. Mag2Car 拦截 → 全程任务到 goalSite
 6. 到达后 GoSite 清理 goalSite / occupied / loopAssigned
 7. 下一轮任务匹配
 
@@ -1462,13 +1462,13 @@ Fass2_Obstacle = 2
 
    
 
  • **不需要改 Core**；LoopMission 逻辑完全沿用
    • 
 
  • 磁导航FASS2环线 仅为场景标识，行为与 LoopMission 相同
    • 
-
  • 真正区别在 **车型** 是否 MagFass2Car 且 EnableLoopTaskDrive=true
    • 
+
  • 真正区别在 **车型** 是否 Mag2Car 且 EnableLoopTaskDrive=true
    • 
 
9.4 关闭 Loop 拦截（恢复多段脚本）

 EnableLoopTaskDrive = false
 

-
Loop 仍走 Compile 多行 MagFass2Go，每行一段边。

+
Loop 仍走 Compile 多行 Mag2Go，每行一段边。
10. 任务状态机与协议下发
10.1 状态流转

@@ -1595,7 +1595,7 @@ Idle → Planning → Moving → AtStation → SegmentDone → … → Complete
 

 

 

 

 车辆开关MagFass2Car.UseTagValueAsNode = trueMag2Car.UseTagValueAsNode = true
 

 

 示例Magnet
 bool
 —true 启用 MagneticTrackCoder（agv.MagGo 等），与 MagFass2Go 模板独立true 启用 MagneticTrackCoder（agv.MagGo 等），与 Mag2Go 模板独立
 

 

 ReverseDst
 
 
 
 
 

 
 

-
日志前缀：[MagFass2Car:车名(id)]，任务状态机：task-sm。

+
日志前缀：[Mag2Car:车名(id)]，任务状态机：task-sm。

 

 
12. 常见问题排查

 
Q1：车显示离线（UDP）

@@ -1698,13 +1698,13 @@ Idle → Planning → Moving → AtStation → SegmentDone → … → Complete
 
看日志：

 
    
 
  • loop goal drive intercept → 全程任务（阶段 4）
    • 
-
  • script begin + 多行 MagFass2Go → 多段脚本
    • 
+
  • script begin + 多行 Mag2Go → 多段脚本
    • 
 

 
Q6：与 MagCar 混用注意事项

 
    
 
  • EventCarMission **只控制 MagCar**
    • 
 
  • 同场景两种车型时，tasklist 和车辆类型要对应
    • 
-
  • FASS 1.0 用 MagCar，FASS 2.0 用 MagFass2Car
    • 
+
  • FASS 1.0 用 MagCar，FASS 2.0 用 Mag2Car
    • 
 

 

 
13. 参数推荐与检查清单

@@ -1722,7 +1722,7 @@ MoveTimeoutSeconds    = 120
 
13.2 上线前检查清单

 
    
 
  • [ ] 插件 StandardScene.Magnetic.dll 已加载
    • 
-
  • [ ] 车型为 MagFass2Car（不是 MagCar）
    • 
+
  • [ ] 车型为 Mag2Car（不是 MagCar）
    • 
 
  • [ ] VehicleCode、IP、端口与车体一致
    • 
 
  • [ ] UDP 能收到 100B，日志有 0x10 应答
    • 
 
  • [ ] 关键站点 TagValue / Fass2_* 已配置
    • 
@@ -1785,7 +1785,7 @@ MoveTimeoutSeconds    = 120
 
 
 车型
-CarTypes/MagFass2Car.cs
+CarTypes/Mag2Car.cs
 
 
 协议
@@ -1805,7 +1805,7 @@ MoveTimeoutSeconds    = 120
 
 
 环线 Mission
-Chained/MagFass2LoopMission.cs
+Chained/Mag2LoopMission.cs
 
 
 站点字段常量
diff --git a/StandardScene.Magnetic/docs/MagFass2Car使用指南.md b/StandardScene.Magnetic/docs/Mag2Car使用指南.md
similarity index 86%
rename from StandardScene.Magnetic/docs/MagFass2Car使用指南.md
rename to StandardScene.Magnetic/docs/Mag2Car使用指南.md
index 308521e..972c6b4 100644
--- a/StandardScene.Magnetic/docs/MagFass2Car使用指南.md
+++ b/StandardScene.Magnetic/docs/Mag2Car使用指南.md
@@ -1,8 +1,8 @@
-# MagFass2Car（FASS 2.0）使用指南
+# Mag2Car（FASS 2.0）使用指南
 
 > 适用对象：第一次接触磁导航 + FASS 2.0 的调试/实施人员  
 > 插件：`StandardScene.Magnetic.dll`  
-> 车型：`MagFass2Car`  
+> 车型：`Mag2Car`  
 > 最后更新：2026-07
 
 ---
@@ -33,7 +33,7 @@
 
 ## 1. 这套系统能做什么
 
-`MagFass2Car` 是 StandardScene **磁导航平台插件**里的 FASS 2.0 车型，主要能力：
+`Mag2Car` 是 StandardScene **磁导航平台插件**里的 FASS 2.0 车型，主要能力：
 
 | 能力 | 说明 |
 |------|------|
@@ -54,12 +54,12 @@
 
 确认场景已加载 `scene.mag`（`StandardScene.Magnetic.scene.json`），提供：
 
-- 车型：`MagCar`、`MagFass2Car`
-- 进程：`EventCarMission`、`MagFass2LoopMission`
+- 车型：`MagCar`、`Mag2Car`
+- 进程：`EventCarMission`、`Mag2LoopMission`
 
 ### 步骤 2：添加一辆车
 
-1. 在场景中添加车型 **`MagFass2Car`**
+1. 在场景中添加车型 **`Mag2Car`**
 2. 填写基本参数（UDP 模式示例）：
 
 | 参数 | 示例值 | 含义 |
@@ -86,8 +86,8 @@
 用系统自带的「去某站」功能，内部会编译脚本，例如：
 
 ```text
-agv.MagFass2Go(1, 2, 0.2);
-agv.MagFass2Go(2, 5, 0.2);
+agv.Mag2Go(1, 2, 0.2);
+agv.Mag2Go(2, 5, 0.2);
 ```
 
 **方式 B — LoopMission 环线**  
@@ -103,11 +103,11 @@ agv.MagFass2Go(2, 5, 0.2);
 ```text
 ┌─────────────────────────────────────────────────────────┐
 │  业务层：LoopMission / 手动 Go / 脚本                    │
-│  （分配 goalSite 或编译多行 MagFass2Go）                  │
+│  （分配 goalSite 或编译多行 Mag2Go）                  │
 └────────────────────────┬────────────────────────────────┘
                          ▼
 ┌─────────────────────────────────────────────────────────┐
-│  MagFass2Car 执行层                                      │
+│  Mag2Car 执行层                                      │
 │  · 脚本拦截（Loop + goalSite）→ 一次全程任务              │
 │  · Fass2TaskBuilder：路径 + fields → 节点序列             │
 │  · Fass2TaskStateMachine：滑动窗口下发 + 动作闭环         │
@@ -125,7 +125,7 @@ agv.MagFass2Go(2, 5, 0.2);
 **关键理解：**
 
 - **LoopMission（Core）** 只负责「给谁分配哪个目标站」（写 `goalSite` 标签）
-- **MagFass2Car** 负责「怎么走过去」（拼包、下发、等到站）
+- **Mag2Car** 负责「怎么走过去」（拼包、下发、等到站）
 - **阶段 4** 后，Loop 不再逐行执行多段脚本，而是**拦截为一次全程任务**（可关）
 
 ---
@@ -138,8 +138,8 @@ agv.MagFass2Go(2, 5, 0.2);
 
 ```json
 {
-  "carTypes": [ "MagCar", "MagFass2Car" ],
-  "missionTypes": [ "EventCarMission", "MagFass2LoopMission" ]
+  "carTypes": [ "MagCar", "Mag2Car" ],
+  "missionTypes": [ "EventCarMission", "Mag2LoopMission" ]
 }
 ```
 
@@ -147,11 +147,11 @@ agv.MagFass2Go(2, 5, 0.2);
 
 | Mission | 来源 | 适用 |
 |---------|------|------|
-| `LoopMission` | Core | 通用环线，MagFass2Car 同样可用 |
-| `磁导航FASS2环线`（`MagFass2LoopMission`） | Magnetic 插件 | 逻辑与 LoopMission 相同，名称标识磁导航 FASS2 场景 |
-| `FASS车辆事件进程`（`EventCarMission`） | Magnetic 插件 | **仅 MagCar（FASS 1.0）**，不驱动 MagFass2Car |
+| `LoopMission` | Core | 通用环线，Mag2Car 同样可用 |
+| `磁导航FASS2环线`（`Mag2LoopMission`） | Magnetic 插件 | 逻辑与 LoopMission 相同，名称标识磁导航 FASS2 场景 |
+| `FASS车辆事件进程`（`EventCarMission`） | Magnetic 插件 | **仅 MagCar（FASS 1.0）**，不驱动 Mag2Car |
 
-> 新手建议：MagFass2Car 环线用 **`LoopMission`** 或 **`磁导航FASS2环线`** 均可。
+> 新手建议：Mag2Car 环线用 **`LoopMission`** 或 **`磁导航FASS2环线`** 均可。
 
 ### 4.3 tasklist.json 最小示例
 
@@ -186,7 +186,7 @@ agv.MagFass2Go(2, 5, 0.2);
 
 ## 5. 车辆参数说明
 
-在场景编辑器中选中 `MagFass2Car`，常用字段如下。
+在场景编辑器中选中 `Mag2Car`，常用字段如下。
 
 ### 5.1 通讯
 
@@ -269,8 +269,8 @@ agv.MagFass2Go(2, 5, 0.2);
 
 ```text
 FindRoute → Compile → 多行脚本：
-  agv.MagFass2Go(1,2,speed);
-  agv.MagFass2Go(2,3,speed);
+  agv.Mag2Go(1,2,speed);
+  agv.Mag2Go(2,3,speed);
   ...
 → SelfEvaluating 逐行执行
 → 每行：锁终点 → 发 0xB1 → 等到站 → 释放起点
@@ -282,7 +282,7 @@ FindRoute → Compile → 多行脚本：
 
 ### 方式 2：Loop 全程任务（阶段 4，默认）
 
-**触发**：`goalSite` 标签 + 编译脚本含 `MagFass2Go` + `EnableLoopTaskDrive=true`。
+**触发**：`goalSite` 标签 + 编译脚本含 `Mag2Go` + `EnableLoopTaskDrive=true`。
 
 **流程**：
 
@@ -298,7 +298,7 @@ LoopMission 写 goalSite
 
 ---
 
-### 方式 3：单段 MagFass2Go（脚本一行）
+### 方式 3：单段 Mag2Go（脚本一行）
 
 **触发**：脚本里只有一行，或无 `goalSite` 的测试。
 
@@ -370,7 +370,7 @@ LoopMission 写 goalSite
 |------|------|
 | 映射协议字段 | `Node`（节点号） |
 | 何时配置 | 磁导航地标号与地图 `site.id` **不一致**时必须配置 |
-| 车辆开关 | `MagFass2Car.UseTagValueAsNode = true` |
+| 车辆开关 | `Mag2Car.UseTagValueAsNode = true` |
 | 示例 | `TagValue = 10086` |
 | 注意 | 未配置且 `UseTagValueAsNode=false` 时，直接用 `site.id` 作为节点号 |
 
@@ -387,9 +387,9 @@ LoopMission 写 goalSite
 
 | 项目 | 说明 |
 |------|------|
-| 映射协议字段 | 无（当前版本不直接写入节点块） |
+| 映射协议字段 | 无（不写入节点块） |
 | 规划值 | `None` / `Button` / `Plc` / `Api` |
-| 当前状态 | 字段已定义，**等待放行联动逻辑待扩展**；可通过修改 `Fass2_StartStop` 或外部改 fields 触发重发 |
+| 当前状态 | 人工/外部放行源预留。**管控区按车放行**请用 `Fass2_StartStop=12` + `Fass2_ControlArea`（站点列表），不要靠改地图 `Fass2_StartStop` 放行 |
 
 ---
 
@@ -401,8 +401,8 @@ LoopMission 写 goalSite
 |----|---------------------------|----------|
 | `1` | 过站（不停） | 路径中间站、通道站 |
 | `2` | 普通停车 | 工位、等待点、装卸点 |
-| `11` | 管控启动 | 管控区入口，需授权后启动 |
-| `12` | 管控停止 | 管控区停车等待 |
+| `11` | 管控启动 | 本车放行后写入 **0xA1 / 后续 0xB1**（地图站点仍保持 12） |
+| `12` | 管控停止 | 管控区停车，等调度按车放行 |
 | `22` | 精准停止 | 装配、对接，精度要求高 |
 
 **默认规则**（未显式配置 `Fass2_StartStop` 时，`Fass2TaskBuilder` 自动决定）：
@@ -413,7 +413,17 @@ LoopMission 写 goalSite
 | 末站（goalSite） | `2`（停车） |
 | 末站 + `Fass2_PrecisionStop=true` | `22`（精准停） |
 
-**动作完成判定**：若配置了非过站值，状态机要求上报 `Node.StartStop` 与期望值一致，且车体非运行态（`State≠1`），才推进下一段。
+**动作完成判定**：`2`/`22` 要求上报 `StartStop` 一致且车体非运行态。`12` **不会**因上报 12 就推进：必须等同区占用条件满足后，调度把本车期望值改为 `11`（或 `1`），车体回显后再离站。
+
+**管控停止按车放行（`12`）**：
+
+- 地图 `Fass2_StartStop` **始终保持 12**，不要改成 1 来放行（否则下一辆车的 0xB1 也会变成过站）。
+- `0xB1` 窗口截断在第一个尚未放行的 12，其后站点不会出现在同一帧里。
+- 本车到站并停稳后，统计 `Fass2_ControlArea` **站点列表**上的他车占用（`holdingLocks` + Mag2 `siteID`）。他车数 **≥** `Fass2_ControlCapacity`（默认 1，即数量与容量相等）则不放行；本车不计入，避免停在列表内时永远走不了。
+- 放行只改**本车**报文：`0xA1` 把当前节点 `12→11`（可用 `Fass2_ControlReleaseStartStop=1` 改为过站），并补发含后续站的 `0xB1`。下一辆车仍会收到 12。
+- 等待放行期间不消耗移动超时。断线重连若仍停在该管控点，会重新判定占用；条件满足则再次发放行报文。
+
+相关站点字段：`Fass2_ControlArea`（站点 ID 数组，如 `10,11,12`）、`Fass2_ControlCapacity`、`Fass2_ControlReleaseStartStop`。未配或解析不出站点时，只统计当前管控点本身。
 
 **与 `Fass2_PrecisionStop` 关系**：
 
@@ -614,6 +624,9 @@ LoopMission 写 goalSite
 | `Fass2_Tray` | byte | `Tray` | 是（非 0 时） |
 | `Fass2_Shutdown` | byte | `Shutdown` | 是（非 0 时） |
 | `Fass2_WaitMode` | string | — | 预留 |
+| `Fass2_ControlArea` | string | — | 管控站点列表，如 `10,11,12` |
+| `Fass2_ControlCapacity` | int | — | 列表内他车数达到该值则不放行，默认 1 |
+| `Fass2_ControlReleaseStartStop` | byte | 放行后的 `StartStop` | `11`（默认）或 `1` |
 
 ---
 
@@ -632,7 +645,7 @@ LoopMission 写 goalSite
 | `magSelect` | byte | `Byroad` | 磁导航 coder 分叉序号；`Fass2_Byroad` 为空时作为 Byroad 备选 |
 | `Fass2_Direction` | byte | `Direction` | 边级车头旋转；站点未覆盖时生效 |
 | `Fass2_Orientation` | byte | `Orientation` | 边级朝向；站点未覆盖时生效 |
-| `Magnet` | bool | — | `true` 启用 `MagneticTrackCoder`（`agv.MagGo` 等），与 `MagFass2Go` 模板独立 |
+| `Magnet` | bool | — | `true` 启用 `MagneticTrackCoder`（`agv.MagGo` 等），与 `Mag2Go` 模板独立 |
 | `ReverseDst` | int | — | 倒车目的地站 ID；磁 coder 跳过该边 |
 
 **距离 `Distance`**：由地图坐标自动计算（上一站与本站欧氏距离，mm 取整），一般无需手配。
@@ -717,6 +730,21 @@ Fass2_Charge = 2      # 开始充电
 
 ---
 
+#### 场景 E2：管控停止（按车放行）
+
+在 12 站点填写要监控的站点列表；**地图 `Fass2_StartStop` 保持 12**。
+
+```text
+Fass2_StartStop = 12
+Fass2_ControlArea = 10,11,12       # 这些站上有他车且数量达到容量则不放行
+Fass2_ControlCapacity = 1
+# Fass2_ControlReleaseStartStop = 11  # 默认；改 1 则放行后按过站离站
+```
+
+列表 `10,11,12` 上已有 1 辆他车时，本车停在 12 等待；他车离开列表后，调度只给本车发 `0xA1`（12→11）。下一辆车仍会先收到 12。
+
+---
+
 #### 场景 F：Y 型磁分叉口
 
 **站点**（推荐在岔口站配）：
@@ -792,7 +820,7 @@ Fass2_Obstacle = 2
 2. 匹配 tasklist：currentStationId = N → 分配 targetStationId
 3. 写 tags：goalSite = 目标站 ID
 4. LoopStartAction 调用 GoSite
-5. MagFass2Car 拦截 → 全程任务到 goalSite
+5. Mag2Car 拦截 → 全程任务到 goalSite
 6. 到达后 GoSite 清理 goalSite / occupied / loopAssigned
 7. 下一轮任务匹配
 ```
@@ -809,7 +837,7 @@ Fass2_Obstacle = 2
 
 - **不需要改 Core**；`LoopMission` 逻辑完全沿用
 - `磁导航FASS2环线` 仅为场景标识，行为与 `LoopMission` 相同
-- 真正区别在 **车型** 是否 `MagFass2Car` 且 `EnableLoopTaskDrive=true`
+- 真正区别在 **车型** 是否 `Mag2Car` 且 `EnableLoopTaskDrive=true`
 
 ### 9.4 关闭 Loop 拦截（恢复多段脚本）
 
@@ -817,7 +845,7 @@ Fass2_Obstacle = 2
 EnableLoopTaskDrive = false
 ```
 
-Loop 仍走 Compile 多行 `MagFass2Go`，每行一段边。
+Loop 仍走 Compile 多行 `Mag2Go`，每行一段边。
 
 ---
 
@@ -890,7 +918,7 @@ Idle → Planning → Moving → AtStation → SegmentDone → … → Complete
 | 重置UDP监听 | — | 重新注册 UDP 会话 |
 | 重置TCP连接 | — | 关闭长连接（TCP 模式） |
 
-日志前缀：`[MagFass2Car:车名(id)]`，任务状态机：`task-sm`。
+日志前缀：`[Mag2Car:车名(id)]`，任务状态机：`task-sm`。
 
 ---
 
@@ -936,13 +964,13 @@ Idle → Planning → Moving → AtStation → SegmentDone → … → Complete
 看日志：
 
 - `loop goal drive intercept` → 全程任务（阶段 4）
-- `script begin` + 多行 `MagFass2Go` → 多段脚本
+- `script begin` + 多行 `Mag2Go` → 多段脚本
 
 ### Q6：与 MagCar 混用注意事项
 
 - `EventCarMission` **只控制 MagCar**
 - 同场景两种车型时，tasklist 和车辆类型要对应
-- FASS 1.0 用 `MagCar`，FASS 2.0 用 `MagFass2Car`
+- FASS 1.0 用 `MagCar`，FASS 2.0 用 `Mag2Car`
 
 ---
 
@@ -964,7 +992,7 @@ MoveTimeoutSeconds    = 120
 ### 13.2 上线前检查清单
 
 - [ ] 插件 `StandardScene.Magnetic.dll` 已加载
-- [ ] 车型为 `MagFass2Car`（不是 `MagCar`）
+- [ ] 车型为 `Mag2Car`（不是 `MagCar`）
 - [ ] `VehicleCode`、IP、端口与车体一致
 - [ ] UDP 能收到 100B，日志有 `0x10` 应答
 - [ ] 关键站点 `TagValue` / `Fass2_*` 已配置
@@ -993,12 +1021,12 @@ MoveTimeoutSeconds    = 120
 
 | 模块 | 路径 |
 |------|------|
-| 车型 | `CarTypes/MagFass2Car.cs` |
+| 车型 | `CarTypes/Mag2Car.cs` |
 | 协议 | `Protocol/Fass2Protocol.cs`、`Fass2UdpHub.cs` |
 | 组包 | `Tasking/Fass2TaskBuilder.cs` |
 | 状态机 | `Tasking/Fass2TaskStateMachine.cs` |
 | 动作判定 | `Tasking/Fass2ActionResolver.cs` |
-| 环线 Mission | `Chained/MagFass2LoopMission.cs` |
+| 环线 Mission | `Chained/Mag2LoopMission.cs` |
 | 站点字段常量 | `Tasking/Fass2SiteFields.cs` |
 
 ---
diff --git a/StandardScene.Magnetic/docs/MagFass2Car使用指南.pdf b/StandardScene.Magnetic/docs/Mag2Car使用指南.pdf
similarity index 100%
rename from StandardScene.Magnetic/docs/MagFass2Car使用指南.pdf
rename to StandardScene.Magnetic/docs/Mag2Car使用指南.pdf
diff --git a/StandardScene.Magnetic/docs/export-pdf.ps1 b/StandardScene.Magnetic/docs/export-pdf.ps1
index fd72c90..c0ea031 100644
--- a/StandardScene.Magnetic/docs/export-pdf.ps1
+++ b/StandardScene.Magnetic/docs/export-pdf.ps1
@@ -1,7 +1,7 @@
-# Export MagFass2Car guide Markdown to PDF (requires Chrome or Edge)
+# Export Mag2Car guide Markdown to PDF (requires Chrome or Edge)
 param(
-    [string]$InputMd = "$PSScriptRoot\MagFass2Car使用指南.md",
-    [string]$OutputPdf = "$PSScriptRoot\MagFass2Car使用指南.pdf"
+    [string]$InputMd = "$PSScriptRoot\Mag2Car使用指南.md",
+    [string]$OutputPdf = "$PSScriptRoot\Mag2Car使用指南.pdf"
 )
 
 $ErrorActionPreference = 'Stop'
@@ -147,7 +147,7 @@ $html = @"
 
 
 
-MagFass2Car Guide
+Mag2Car Guide