磁导航1.0内部交管和信号交互
This commit is contained in:
ykkokluo
@@ -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