磁导航1.0内部交管和信号交互
This commit is contained in:
ykkokluo
@@ -0,0 +1,114 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace StandardScene.MagCarSimulator
|
||||
{
|
||||
public sealed class MagCarSimVehicleConfig
|
||||
{
|
||||
public string Name { get; set; } = "AGV";
|
||||
public ushort VehicleCode { get; set; } = 1;
|
||||
public int ListenPort { get; set; } = 5001;
|
||||
public int StartSiteId { get; set; } = 1;
|
||||
public int EndSiteId { get; set; } = 2;
|
||||
public int IntervalMs { get; set; } = 2000;
|
||||
|
||||
/// <summary>显式环线站点序列（不含回到起点的重复点）。有值时优先于起点/终点寻路。</summary>
|
||||
public List<int> LoopSiteIds { get; set; }
|
||||
}
|
||||
|
||||
public sealed class MagCarSimConfig
|
||||
{
|
||||
public string MapPath { get; set; } = "";
|
||||
public bool UseTagValueAsNode { get; set; }
|
||||
public string ListenAddress { get; set; } = "0.0.0.0";
|
||||
public bool LogRawFrames { get; set; }
|
||||
public bool LogQueries { get; set; }
|
||||
|
||||
/// <summary>文件日志根目录（相对 exe 或绝对路径），默认 log。</summary>
|
||||
public string LogDirectory { get; set; } = "log";
|
||||
|
||||
/// <summary>是否写入 log 目录下的 magcar-sim_yyyyMMdd.log。</summary>
|
||||
public bool EnableFileLog { get; set; } = true;
|
||||
|
||||
public int DefaultIntervalMs { get; set; } = 2000;
|
||||
|
||||
/// <summary>同一环线后车相对前车的发车间隔。≤0 时用该车 IntervalMs。</summary>
|
||||
public int LaunchStaggerMs { get; set; }
|
||||
|
||||
/// <summary>磁条交管表。空则按地图目录推断 SimpleLite/config/Signal/mag-control-areas.json。</summary>
|
||||
public string ControlAreasPath { get; set; }
|
||||
public List<MagCarSimVehicleConfig> Vehicles { get; set; } = new List<MagCarSimVehicleConfig>();
|
||||
|
||||
public static string SettingsPath => Path.Combine(AppContext.BaseDirectory, "appsettings.json");
|
||||
|
||||
public static MagCarSimConfig Load(string path = null)
|
||||
{
|
||||
path ??= SettingsPath;
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
return CreateDefault();
|
||||
}
|
||||
|
||||
var json = File.ReadAllText(path);
|
||||
var config = JsonConvert.DeserializeObject<MagCarSimConfig>(json) ?? CreateDefault();
|
||||
if (config.Vehicles == null || config.Vehicles.Count == 0)
|
||||
{
|
||||
config.Vehicles = CreateDefault().Vehicles;
|
||||
}
|
||||
|
||||
if (config.DefaultIntervalMs <= 0)
|
||||
{
|
||||
config.DefaultIntervalMs = 2000;
|
||||
}
|
||||
|
||||
foreach (var vehicle in config.Vehicles)
|
||||
{
|
||||
if (vehicle.IntervalMs <= 0)
|
||||
{
|
||||
vehicle.IntervalMs = config.DefaultIntervalMs;
|
||||
}
|
||||
|
||||
if (string.IsNullOrWhiteSpace(vehicle.Name))
|
||||
{
|
||||
vehicle.Name = $"AGV-{vehicle.VehicleCode}";
|
||||
}
|
||||
|
||||
if (vehicle.LoopSiteIds != null && vehicle.LoopSiteIds.Count > 0)
|
||||
{
|
||||
vehicle.LoopSiteIds = MapPathFinder.NormalizeLoop(vehicle.LoopSiteIds);
|
||||
}
|
||||
}
|
||||
|
||||
return config;
|
||||
}
|
||||
|
||||
public void Save(string path = null)
|
||||
{
|
||||
path ??= SettingsPath;
|
||||
var json = JsonConvert.SerializeObject(this, Formatting.Indented);
|
||||
File.WriteAllText(path, json);
|
||||
}
|
||||
|
||||
public static MagCarSimConfig CreateDefault()
|
||||
{
|
||||
return new MagCarSimConfig
|
||||
{
|
||||
DefaultIntervalMs = 2000,
|
||||
Vehicles =
|
||||
{
|
||||
new MagCarSimVehicleConfig
|
||||
{
|
||||
Name = "AGV-1",
|
||||
VehicleCode = 1,
|
||||
ListenPort = 5001,
|
||||
StartSiteId = 1,
|
||||
EndSiteId = 6,
|
||||
IntervalMs = 2000
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,335 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
|
||||
namespace StandardScene.MagCarSimulator
|
||||
{
|
||||
public sealed class MagCarSimHost : IDisposable
|
||||
{
|
||||
private readonly object _syncRoot = new object();
|
||||
private readonly List<MagCarSimVehicle> _vehicles = new List<MagCarSimVehicle>();
|
||||
private Timer _timer;
|
||||
private SimpleLiteMap _map;
|
||||
private int _tickBusy;
|
||||
|
||||
public MagCarSimHost(MagCarSimConfig config)
|
||||
{
|
||||
Config = config ?? throw new ArgumentNullException(nameof(config));
|
||||
}
|
||||
|
||||
public MagCarSimConfig Config { get; }
|
||||
public SimpleLiteMap Map => _map;
|
||||
public bool IsListening { get; private set; }
|
||||
public bool IsLooping { get; private set; }
|
||||
|
||||
public IReadOnlyList<MagCarSimVehicle> Vehicles
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return _vehicles.ToList();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public SimpleLiteMap LoadMap(string path)
|
||||
{
|
||||
var map = SimpleLiteMap.Load(path, Config.UseTagValueAsNode);
|
||||
var controlPath = MagControlAreaFile.GuessPath(map.FilePath, Config.ControlAreasPath);
|
||||
var controlSites = MagControlAreaFile.LoadSiteIds(controlPath);
|
||||
map.AddSingleOccupancySites(controlSites);
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_map = map;
|
||||
Config.MapPath = map.FilePath;
|
||||
if (!string.IsNullOrWhiteSpace(controlPath))
|
||||
{
|
||||
Config.ControlAreasPath = controlPath;
|
||||
}
|
||||
|
||||
foreach (var vehicle in _vehicles)
|
||||
{
|
||||
vehicle.AttachMap(map);
|
||||
}
|
||||
}
|
||||
|
||||
var exclusive = map.ListSingleOccupancySiteIds();
|
||||
MagCarSimLog.WriteLine(
|
||||
$"[{DateTime.Now:HH:mm:ss.fff}] 已加载地图 {map.FileName}，站点 {map.Sites.Count}，路径 {map.Tracks.Count}，停止/交管点 {(exclusive.Count == 0 ? "无" : string.Join(",", exclusive))}");
|
||||
return map;
|
||||
}
|
||||
|
||||
public void RebuildVehicles(IEnumerable<MagCarSimVehicleConfig> configs)
|
||||
{
|
||||
if (IsListening)
|
||||
{
|
||||
throw new InvalidOperationException("请先停止模拟再改车辆列表");
|
||||
}
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
DisposeVehiclesUnlocked();
|
||||
foreach (var config in configs ?? Array.Empty<MagCarSimVehicleConfig>())
|
||||
{
|
||||
if (config == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var vehicle = new MagCarSimVehicle(Clone(config), Config);
|
||||
if (_map != null)
|
||||
{
|
||||
vehicle.AttachMap(_map);
|
||||
}
|
||||
|
||||
_vehicles.Add(vehicle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void StartListen()
|
||||
{
|
||||
List<MagCarSimVehicle> vehicles;
|
||||
SimpleLiteMap map;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (IsListening)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_map == null)
|
||||
{
|
||||
throw new InvalidOperationException("请先加载 SimpleLite 地图");
|
||||
}
|
||||
|
||||
if (_vehicles.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException("没有可启动的车辆");
|
||||
}
|
||||
|
||||
EnsureUniquePortsUnlocked();
|
||||
vehicles = _vehicles.ToList();
|
||||
map = _map;
|
||||
EnsureTimerUnlocked();
|
||||
IsListening = true;
|
||||
}
|
||||
|
||||
foreach (var vehicle in vehicles)
|
||||
{
|
||||
vehicle.AttachMap(map);
|
||||
vehicle.StartListen();
|
||||
}
|
||||
}
|
||||
|
||||
public void StartLoops()
|
||||
{
|
||||
if (!IsListening)
|
||||
{
|
||||
StartListen();
|
||||
}
|
||||
|
||||
List<MagCarSimVehicle> vehicles;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
vehicles = _vehicles.ToList();
|
||||
IsLooping = true;
|
||||
}
|
||||
|
||||
var now = DateTime.Now;
|
||||
foreach (var group in vehicles.GroupBy(RingKey).OrderBy(g => g.Key))
|
||||
{
|
||||
var members = group.OrderBy(v => v.VehicleCode).ToList();
|
||||
var dispatchAt = now;
|
||||
foreach (var vehicle in members)
|
||||
{
|
||||
vehicle.StartLoop(dispatchAt);
|
||||
dispatchAt = dispatchAt.AddMilliseconds(StaggerMs(vehicle));
|
||||
}
|
||||
|
||||
MagCarSimLog.WriteLine(
|
||||
$"[{DateTime.Now:HH:mm:ss.fff}] 同环依次发车 {members[0].Name} 等 {members.Count} 台，间隔 {StaggerMs(members[0])}ms");
|
||||
}
|
||||
}
|
||||
|
||||
public void StopAll()
|
||||
{
|
||||
Timer timer;
|
||||
List<MagCarSimVehicle> vehicles;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
timer = _timer;
|
||||
_timer = null;
|
||||
IsListening = false;
|
||||
IsLooping = false;
|
||||
vehicles = _vehicles.ToList();
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
timer?.Dispose();
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
|
||||
foreach (var vehicle in vehicles)
|
||||
{
|
||||
vehicle.StopLoop();
|
||||
vehicle.StopListen();
|
||||
}
|
||||
}
|
||||
|
||||
public bool TryRelease(ushort vehicleCode)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
var vehicle = _vehicles.FirstOrDefault(v => v.VehicleCode == vehicleCode);
|
||||
if (vehicle == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
vehicle.Release();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
public List<MagCarSimVehicleSnapshot> Snapshots()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return _vehicles.Select(v => v.Snapshot()).ToList();
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
StopAll();
|
||||
lock (_syncRoot)
|
||||
{
|
||||
DisposeVehiclesUnlocked();
|
||||
}
|
||||
}
|
||||
|
||||
private void EnsureTimerUnlocked()
|
||||
{
|
||||
_timer ??= new Timer(_ => OnTick(), null, 50, 50);
|
||||
}
|
||||
|
||||
private void OnTick()
|
||||
{
|
||||
if (Interlocked.Exchange(ref _tickBusy, 1) != 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
List<MagCarSimVehicle> vehicles;
|
||||
SimpleLiteMap map;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
vehicles = _vehicles.ToList();
|
||||
map = _map;
|
||||
}
|
||||
|
||||
var occupancy = SnapshotExclusiveOccupancy(map, vehicles);
|
||||
var now = DateTime.Now;
|
||||
foreach (var vehicle in vehicles)
|
||||
{
|
||||
vehicle.Tick(now, occupancy);
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
Interlocked.Exchange(ref _tickBusy, 0);
|
||||
}
|
||||
}
|
||||
|
||||
private void EnsureUniquePortsUnlocked()
|
||||
{
|
||||
var seen = new HashSet<int>();
|
||||
foreach (var vehicle in _vehicles)
|
||||
{
|
||||
if (!seen.Add(vehicle.ListenPort))
|
||||
{
|
||||
throw new InvalidOperationException($"端口 {vehicle.ListenPort} 被多辆车占用");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static Dictionary<int, ushort> SnapshotExclusiveOccupancy(
|
||||
SimpleLiteMap map,
|
||||
IEnumerable<MagCarSimVehicle> vehicles)
|
||||
{
|
||||
var occupancy = new Dictionary<int, ushort>();
|
||||
if (map == null)
|
||||
{
|
||||
return occupancy;
|
||||
}
|
||||
|
||||
foreach (var vehicle in vehicles)
|
||||
{
|
||||
if (vehicle.CurrentSiteId <= 0 ||
|
||||
!map.SingleOccupancy(vehicle.CurrentSiteId) ||
|
||||
occupancy.ContainsKey(vehicle.CurrentSiteId))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
occupancy[vehicle.CurrentSiteId] = vehicle.VehicleCode;
|
||||
}
|
||||
|
||||
return occupancy;
|
||||
}
|
||||
|
||||
private int StaggerMs(MagCarSimVehicle vehicle)
|
||||
{
|
||||
if (Config.LaunchStaggerMs > 0)
|
||||
{
|
||||
return Math.Max(200, Config.LaunchStaggerMs);
|
||||
}
|
||||
|
||||
var interval = vehicle.Config.IntervalMs > 0 ? vehicle.Config.IntervalMs : Config.DefaultIntervalMs;
|
||||
return Math.Max(200, interval);
|
||||
}
|
||||
|
||||
private static string RingKey(MagCarSimVehicle vehicle)
|
||||
{
|
||||
var ids = vehicle.Config.LoopSiteIds;
|
||||
if (ids != null && ids.Count > 0)
|
||||
{
|
||||
return string.Join("-", ids);
|
||||
}
|
||||
|
||||
return $"s{vehicle.Config.StartSiteId}-e{vehicle.Config.EndSiteId}";
|
||||
}
|
||||
|
||||
private void DisposeVehiclesUnlocked()
|
||||
{
|
||||
foreach (var vehicle in _vehicles)
|
||||
{
|
||||
vehicle.Dispose();
|
||||
}
|
||||
|
||||
_vehicles.Clear();
|
||||
}
|
||||
|
||||
private static MagCarSimVehicleConfig Clone(MagCarSimVehicleConfig source)
|
||||
{
|
||||
return new MagCarSimVehicleConfig
|
||||
{
|
||||
Name = source.Name,
|
||||
VehicleCode = source.VehicleCode,
|
||||
ListenPort = source.ListenPort,
|
||||
StartSiteId = source.StartSiteId,
|
||||
EndSiteId = source.EndSiteId,
|
||||
IntervalMs = source.IntervalMs,
|
||||
LoopSiteIds = source.LoopSiteIds == null ? null : new List<int>(source.LoopSiteIds)
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,499 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Net;
|
||||
|
||||
namespace StandardScene.MagCarSimulator
|
||||
{
|
||||
public sealed class MagCarSimVehicleSnapshot
|
||||
{
|
||||
public string Name { get; set; }
|
||||
public ushort VehicleCode { get; set; }
|
||||
public int ListenPort { get; set; }
|
||||
public int StartSiteId { get; set; }
|
||||
public int EndSiteId { get; set; }
|
||||
public int IntervalMs { get; set; }
|
||||
public int CurrentSiteId { get; set; }
|
||||
public ushort Node { get; set; }
|
||||
public byte State { get; set; }
|
||||
public string StateText { get; set; }
|
||||
public bool WaitingRelease { get; set; }
|
||||
public bool HoldingForStop { get; set; }
|
||||
public bool Looping { get; set; }
|
||||
public bool Listening { get; set; }
|
||||
public int ClientCount { get; set; }
|
||||
public string PathText { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 单车循环：普通站可叠车；停止点与交管点同时只允许一辆，后车停在上一站并保持运行中；仅 Mag_NeedStop 停车等 0x01。
|
||||
/// </summary>
|
||||
public sealed class MagCarSimVehicle : IDisposable
|
||||
{
|
||||
private readonly object _syncRoot = new object();
|
||||
private readonly MagCarSimConfig _hostConfig;
|
||||
private MagCarSimTcpServer _server;
|
||||
private SimpleLiteMap _map;
|
||||
private List<int> _loopSiteIds = new List<int>();
|
||||
private int _index;
|
||||
private DateTime _nextHopAt = DateTime.MaxValue;
|
||||
private DateTime _dispatchAt = DateTime.MinValue;
|
||||
private DateTime _lastHoldLogAt = DateTime.MinValue;
|
||||
private bool _looping;
|
||||
private bool _dispatched;
|
||||
private bool _waitingRelease;
|
||||
private bool _pausedByStopCommand;
|
||||
private bool _holdingForStop;
|
||||
|
||||
public MagCarSimVehicle(MagCarSimVehicleConfig config, MagCarSimConfig hostConfig)
|
||||
{
|
||||
Config = config ?? throw new ArgumentNullException(nameof(config));
|
||||
_hostConfig = hostConfig ?? throw new ArgumentNullException(nameof(hostConfig));
|
||||
Name = string.IsNullOrWhiteSpace(config.Name) ? $"AGV-{config.VehicleCode}" : config.Name;
|
||||
Charge = 100;
|
||||
Voltage = 24f;
|
||||
Speed = 50;
|
||||
}
|
||||
|
||||
public MagCarSimVehicleConfig Config { get; }
|
||||
public string Name { get; }
|
||||
public ushort VehicleCode => Config.VehicleCode;
|
||||
public int ListenPort => Config.ListenPort;
|
||||
public int CurrentSiteId { get; private set; }
|
||||
public ushort Node { get; private set; }
|
||||
public byte State { get; private set; }
|
||||
public byte Charge { get; private set; }
|
||||
public float Current { get; private set; }
|
||||
public float Voltage { get; private set; }
|
||||
public byte Speed { get; private set; }
|
||||
public ushort Angle { get; private set; }
|
||||
public byte Task { get; private set; }
|
||||
public byte Lift { get; private set; }
|
||||
public byte Roll { get; private set; }
|
||||
public bool WaitingRelease => _waitingRelease;
|
||||
public bool HoldingForStop => _holdingForStop;
|
||||
public bool Looping => _looping;
|
||||
|
||||
public bool Listening => _server != null && _server.IsRunning;
|
||||
|
||||
public void AttachMap(SimpleLiteMap map)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_map = map;
|
||||
if (!_looping)
|
||||
{
|
||||
PlaceAtStartUnlocked();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void StartListen()
|
||||
{
|
||||
MagCarSimTcpServer server;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (_server != null && _server.IsRunning)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_server?.Dispose();
|
||||
var address = string.IsNullOrWhiteSpace(_hostConfig.ListenAddress)
|
||||
? "0.0.0.0"
|
||||
: _hostConfig.ListenAddress;
|
||||
server = new MagCarSimTcpServer(IPAddress.Parse(address), Config.ListenPort, HandleFrame);
|
||||
_server = server;
|
||||
PlaceAtStartUnlocked();
|
||||
Log($"监听 {address}:{Config.ListenPort}，当前站={CurrentSiteId} node={Node} {MagCarSimProtocol.StateText(State)}");
|
||||
}
|
||||
|
||||
server.Start();
|
||||
}
|
||||
|
||||
public void StopListen()
|
||||
{
|
||||
MagCarSimTcpServer server;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
server = _server;
|
||||
_server = null;
|
||||
}
|
||||
|
||||
server?.Dispose();
|
||||
}
|
||||
|
||||
public void StartLoop(DateTime dispatchAt)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (_map == null)
|
||||
{
|
||||
throw new InvalidOperationException($"{Name} 尚未加载地图");
|
||||
}
|
||||
|
||||
_loopSiteIds = MapPathFinder.ResolveLoop(_map, Config);
|
||||
_index = 0;
|
||||
if (Config.StartSiteId > 0)
|
||||
{
|
||||
var found = _loopSiteIds.IndexOf(Config.StartSiteId);
|
||||
if (found >= 0)
|
||||
{
|
||||
_index = found;
|
||||
}
|
||||
}
|
||||
|
||||
_looping = true;
|
||||
_dispatched = false;
|
||||
_holdingForStop = false;
|
||||
_pausedByStopCommand = false;
|
||||
_waitingRelease = false;
|
||||
_dispatchAt = dispatchAt;
|
||||
PlaceAtStartUnlocked();
|
||||
// 待命阶段若起点是停止点，先报运行中，避免交管误放行并占住 pending，导致真发车后长时间不放。
|
||||
if (_map != null && _map.NeedStop(CurrentSiteId))
|
||||
{
|
||||
State = MagCarSimProtocol.StateRunning;
|
||||
_waitingRelease = false;
|
||||
}
|
||||
|
||||
var delayMs = Math.Max(0, (int)(dispatchAt - DateTime.Now).TotalMilliseconds);
|
||||
Log($"待命发车，环线 {MapPathFinder.FormatPath(_loopSiteIds, 16)}，间隔 {IntervalOrDefault()}ms" +
|
||||
(delayMs > 50 ? $"，{delayMs}ms 后启动" : "，立即启动"));
|
||||
}
|
||||
}
|
||||
|
||||
public void StopLoop()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_looping = false;
|
||||
_dispatched = false;
|
||||
_waitingRelease = false;
|
||||
_holdingForStop = false;
|
||||
_pausedByStopCommand = false;
|
||||
_nextHopAt = DateTime.MaxValue;
|
||||
State = MagCarSimProtocol.StateStopping;
|
||||
Log("已停止循环");
|
||||
}
|
||||
}
|
||||
|
||||
public void Release()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
ApplyStartUnlocked("界面放行");
|
||||
}
|
||||
}
|
||||
|
||||
public void Tick(DateTime now, Dictionary<int, ushort> occupancy)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!_looping || _pausedByStopCommand)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_dispatched)
|
||||
{
|
||||
TryDispatchUnlocked(now, occupancy);
|
||||
return;
|
||||
}
|
||||
|
||||
if (_waitingRelease)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (State != MagCarSimProtocol.StateRunning)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (now < _nextHopAt)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
AdvanceUnlocked(now, occupancy);
|
||||
}
|
||||
}
|
||||
|
||||
public MagCarSimVehicleSnapshot Snapshot()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return new MagCarSimVehicleSnapshot
|
||||
{
|
||||
Name = Name,
|
||||
VehicleCode = VehicleCode,
|
||||
ListenPort = ListenPort,
|
||||
StartSiteId = Config.StartSiteId,
|
||||
EndSiteId = Config.EndSiteId,
|
||||
IntervalMs = Config.IntervalMs,
|
||||
CurrentSiteId = CurrentSiteId,
|
||||
Node = Node,
|
||||
State = State,
|
||||
StateText = MagCarSimProtocol.StateText(State),
|
||||
WaitingRelease = _waitingRelease,
|
||||
HoldingForStop = _holdingForStop,
|
||||
Looping = _looping,
|
||||
Listening = Listening,
|
||||
ClientCount = _server?.ClientCount ?? 0,
|
||||
PathText = MapPathFinder.FormatPath(_loopSiteIds, 12)
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_looping = false;
|
||||
}
|
||||
|
||||
StopListen();
|
||||
}
|
||||
|
||||
private byte[] HandleFrame(byte[] request)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!MagCarSimProtocol.TryParse(request, out var parsed))
|
||||
{
|
||||
Log("收到非法帧");
|
||||
return BuildStatusUnlocked(MagCarSimProtocol.CmdQueryState);
|
||||
}
|
||||
|
||||
if (parsed.VehicleCode != 0 && parsed.VehicleCode != VehicleCode)
|
||||
{
|
||||
Log($"帧车号 {parsed.VehicleCode} 与本车 {VehicleCode} 不一致，仍按本车应答");
|
||||
}
|
||||
|
||||
if (_hostConfig.LogRawFrames)
|
||||
{
|
||||
Log($"RX {MagCarSimProtocol.CommandName(parsed.Command)} {MagCarSimProtocol.ToHex(request)}");
|
||||
}
|
||||
else if (parsed.Command != MagCarSimProtocol.CmdQueryState || _hostConfig.LogQueries)
|
||||
{
|
||||
Log($"RX {MagCarSimProtocol.CommandName(parsed.Command)}");
|
||||
}
|
||||
|
||||
switch (parsed.Command)
|
||||
{
|
||||
case MagCarSimProtocol.CmdStart:
|
||||
ApplyStartUnlocked("0x01");
|
||||
break;
|
||||
case MagCarSimProtocol.CmdStop:
|
||||
_pausedByStopCommand = true;
|
||||
State = MagCarSimProtocol.StateStopping;
|
||||
Log("收到停止，暂停循环");
|
||||
break;
|
||||
case MagCarSimProtocol.CmdSpeed:
|
||||
Speed = (byte)Math.Clamp((int)parsed.Data0 >> 8, 0, 100);
|
||||
break;
|
||||
case MagCarSimProtocol.CmdAngle:
|
||||
Angle = parsed.Data0;
|
||||
break;
|
||||
case MagCarSimProtocol.CmdLegacyNode:
|
||||
case MagCarSimProtocol.CmdTask:
|
||||
Log($"忽略路径命令 {MagCarSimProtocol.CommandName(parsed.Command)}（本模拟器按 WinForm 循环跑）");
|
||||
break;
|
||||
}
|
||||
|
||||
return BuildStatusUnlocked(parsed.Command);
|
||||
}
|
||||
}
|
||||
|
||||
private void ApplyStartUnlocked(string source)
|
||||
{
|
||||
if (!_dispatched)
|
||||
{
|
||||
Log($"待命中忽略放行（{source}）");
|
||||
return;
|
||||
}
|
||||
|
||||
if (_waitingRelease)
|
||||
{
|
||||
_waitingRelease = false;
|
||||
_pausedByStopCommand = false;
|
||||
State = MagCarSimProtocol.StateRunning;
|
||||
_nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
|
||||
Log($"放行成功（{source}），站点 {CurrentSiteId} 改为运行中，{IntervalOrDefault()}ms 后前往下一站");
|
||||
return;
|
||||
}
|
||||
|
||||
if (_pausedByStopCommand && _looping)
|
||||
{
|
||||
_pausedByStopCommand = false;
|
||||
State = MagCarSimProtocol.StateRunning;
|
||||
_nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
|
||||
Log($"从暂停恢复（{source}）");
|
||||
return;
|
||||
}
|
||||
|
||||
if (_looping && State != MagCarSimProtocol.StateRunning)
|
||||
{
|
||||
State = MagCarSimProtocol.StateRunning;
|
||||
_nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
|
||||
Log($"启动运行（{source}）");
|
||||
}
|
||||
}
|
||||
|
||||
private void PlaceAtStartUnlocked()
|
||||
{
|
||||
CurrentSiteId = Config.StartSiteId;
|
||||
Node = _map == null ? (ushort)Math.Clamp(Config.StartSiteId, 0, ushort.MaxValue) : _map.ResolveNode(Config.StartSiteId);
|
||||
State = MagCarSimProtocol.StateStopping;
|
||||
_waitingRelease = false;
|
||||
_pausedByStopCommand = false;
|
||||
}
|
||||
|
||||
private void ArriveUnlocked(int siteId, bool isStart)
|
||||
{
|
||||
CurrentSiteId = siteId;
|
||||
Node = _map.ResolveNode(siteId);
|
||||
var needStop = _map.NeedStop(siteId);
|
||||
if (needStop)
|
||||
{
|
||||
State = MagCarSimProtocol.StateStopping;
|
||||
_waitingRelease = true;
|
||||
_nextHopAt = DateTime.MaxValue;
|
||||
Log($"{(isStart ? "起点" : "到站")} {siteId} node={Node} Mag_NeedStop=true → 停止中，等待放行");
|
||||
return;
|
||||
}
|
||||
|
||||
State = MagCarSimProtocol.StateRunning;
|
||||
_waitingRelease = false;
|
||||
_nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
|
||||
if (isStart)
|
||||
{
|
||||
Log($"起点 {siteId} node={Node} 过站，{IntervalOrDefault()}ms 后下一地标");
|
||||
}
|
||||
}
|
||||
|
||||
private void TryDispatchUnlocked(DateTime now, Dictionary<int, ushort> occupancy)
|
||||
{
|
||||
if (now < _dispatchAt)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_loopSiteIds.Count == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var startSite = _loopSiteIds[_index];
|
||||
if (IsOccupiedUnlocked(startSite, occupancy))
|
||||
{
|
||||
HoldForOccupiedUnlocked(now, startSite, "发车");
|
||||
return;
|
||||
}
|
||||
|
||||
OccupyUnlocked(occupancy, startSite);
|
||||
_dispatched = true;
|
||||
_holdingForStop = false;
|
||||
ArriveUnlocked(startSite, isStart: true);
|
||||
}
|
||||
|
||||
private void AdvanceUnlocked(DateTime now, Dictionary<int, ushort> occupancy)
|
||||
{
|
||||
if (_loopSiteIds.Count < 2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var nextIndex = (_index + 1) % _loopSiteIds.Count;
|
||||
var nextSite = _loopSiteIds[nextIndex];
|
||||
if (IsOccupiedUnlocked(nextSite, occupancy))
|
||||
{
|
||||
HoldForOccupiedUnlocked(now, nextSite, "进站");
|
||||
return;
|
||||
}
|
||||
|
||||
MoveOccupancyUnlocked(occupancy, CurrentSiteId, nextSite);
|
||||
_holdingForStop = false;
|
||||
_index = nextIndex;
|
||||
ArriveUnlocked(nextSite, isStart: false);
|
||||
}
|
||||
|
||||
private bool IsOccupiedUnlocked(int siteId, Dictionary<int, ushort> occupancy)
|
||||
{
|
||||
if (_map == null || occupancy == null || !_map.SingleOccupancy(siteId))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return occupancy.TryGetValue(siteId, out var occupier) && occupier != VehicleCode;
|
||||
}
|
||||
|
||||
private void OccupyUnlocked(Dictionary<int, ushort> occupancy, int siteId)
|
||||
{
|
||||
if (occupancy == null || siteId <= 0 || _map == null || !_map.SingleOccupancy(siteId))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
occupancy[siteId] = VehicleCode;
|
||||
}
|
||||
|
||||
private void MoveOccupancyUnlocked(Dictionary<int, ushort> occupancy, int fromSiteId, int toSiteId)
|
||||
{
|
||||
if (occupancy == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (fromSiteId > 0 &&
|
||||
occupancy.TryGetValue(fromSiteId, out var occupier) &&
|
||||
occupier == VehicleCode)
|
||||
{
|
||||
occupancy.Remove(fromSiteId);
|
||||
}
|
||||
|
||||
OccupyUnlocked(occupancy, toSiteId);
|
||||
}
|
||||
|
||||
private void HoldForOccupiedUnlocked(DateTime now, int occupiedSiteId, string action)
|
||||
{
|
||||
_holdingForStop = true;
|
||||
State = MagCarSimProtocol.StateRunning;
|
||||
_waitingRelease = false;
|
||||
_nextHopAt = now.AddMilliseconds(200);
|
||||
if ((now - _lastHoldLogAt).TotalMilliseconds >= 5000)
|
||||
{
|
||||
_lastHoldLogAt = now;
|
||||
Log($"停止/交管点 {occupiedSiteId} 已有车，{action}受阻，保持站点 {CurrentSiteId} 运行中");
|
||||
}
|
||||
}
|
||||
|
||||
private int IntervalOrDefault()
|
||||
{
|
||||
var value = Config.IntervalMs > 0 ? Config.IntervalMs : _hostConfig.DefaultIntervalMs;
|
||||
return Math.Max(200, value);
|
||||
}
|
||||
|
||||
private byte[] BuildStatusUnlocked(byte command)
|
||||
{
|
||||
return MagCarSimProtocol.BuildStatus(
|
||||
command,
|
||||
VehicleCode,
|
||||
Node,
|
||||
State,
|
||||
Charge,
|
||||
Current,
|
||||
Voltage,
|
||||
Speed,
|
||||
Angle,
|
||||
Task,
|
||||
Lift,
|
||||
Roll);
|
||||
}
|
||||
|
||||
private void Log(string message)
|
||||
{
|
||||
MagCarSimLog.WriteLine($"[{DateTime.Now:HH:mm:ss.fff}] [{Name}/{VehicleCode}] {message}");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user