using Bro.Common.Base;
|
using Bro.Common.Helper;
|
using Bro.Common.Interface;
|
using Bro.Common.Model;
|
using Bro.Common.Model.Interface;
|
using Newtonsoft.Json;
|
using Newtonsoft.Json.Linq;
|
using System;
|
using System.Collections.Generic;
|
using System.Linq;
|
using System.Net;
|
using System.Net.Sockets;
|
using System.Threading;
|
using System.Threading.Tasks;
|
|
namespace Bro.Device.SeerAGV
|
{
|
[Device("SeerAGV", "SeerAGV", EnumHelper.DeviceAttributeType.Device)]
|
public class SeerAGVDriver : DeviceBase, IMonitor
|
{
|
public Action<SeerAGVDriver, string> OnAGVPositoinChanged;
|
public Action<SeerAGVDriver, AGVTaskStatus> OnAGVTaskStatusChanged;
|
public Action<SeerAGVDriver, float, float> OnAGVBatteryLvlChanged;
|
public Action<SeerAGVDriver, IODefinition> OnAGVIOChanged;
|
|
SeerAGVInitialConfig IConfig
|
{
|
get => InitialConfig as SeerAGVInitialConfig;
|
}
|
|
#region DeviceBase
|
protected override void DeviceRun(IOperationConfig config)
|
{
|
}
|
|
protected override void DeviceSet(IDeviceConfig config)
|
{
|
}
|
|
protected override void Init()
|
{
|
InitialTcpClient(ref client_State, IConfig.StatusPort);
|
InitialTcpClient(ref client_Guide, IConfig.GuidePort);
|
|
StateHandle = new ManualResetEvent(false);
|
PositionHandle = new ManualResetEvent(false);
|
BatteryHandle = new ManualResetEvent(false);
|
IOHandle = new ManualResetEvent(false);
|
|
_taskDoneHandle = new ManualResetEvent(false);
|
_monitorLock = new ManualResetEvent(true);
|
_isTaskRunning = false;
|
}
|
|
private void InitialTcpClient(ref TcpClient client, int port)
|
{
|
if (client == null || !client.Connected)
|
{
|
client = new TcpClient();
|
client.SendBufferSize = client.ReceiveBufferSize = 0;
|
client.Client.Blocking = true;
|
client.NoDelay = true;
|
client.Connect(IPAddress.Parse(IConfig.AGVIP), port);
|
}
|
}
|
|
protected override void Pause()
|
{
|
}
|
|
protected override void Resume()
|
{
|
}
|
|
protected override void Start()
|
{
|
msg_Position = new SeerMessage((int)AGVCode.QueryPosition, SID);
|
msg_GuideStatus = new SeerMessage((int)AGVCode.QueryTaskStatus, SID, IConfig.IsSimpleMonitor ? JsonConvert.SerializeObject(new { simple = true }) : "");
|
msg_Battery = new SeerMessage((int)AGVCode.QueryBattery, SID, IConfig.IsSimpleMonitor ? JsonConvert.SerializeObject(new { simple = true }) : "");
|
msg_IO = new SeerMessage((int)AGVCode.QueryIO, SID);
|
|
_taskDoneHandle.Reset();
|
_monitorLock.Set();
|
|
Task.Run(() =>
|
{
|
Monitor();
|
});
|
|
Task.Run(() =>
|
{
|
MonitorAGV();
|
});
|
}
|
|
protected override void Stop()
|
{
|
StateHandle.Set();
|
PositionHandle.Set();
|
BatteryHandle.Set();
|
IOHandle.Set();
|
|
_taskDoneHandle.Set();
|
_monitorLock.Set();
|
|
if (client_Guide != null && client_Guide.Connected)
|
{
|
CancelTask();
|
client_Guide.Close();
|
client_Guide = null;
|
}
|
|
if (client_State != null && client_State.Connected)
|
{
|
client_State.Close();
|
client_State = null;
|
}
|
}
|
#endregion
|
|
int sid = 1;
|
int SID
|
{
|
get
|
{
|
if (sid > 65535)
|
{
|
sid = 1;
|
}
|
|
return sid++;
|
}
|
}
|
|
TcpClient client_State = new TcpClient();
|
TcpClient client_Guide = new TcpClient();
|
byte[] buffer = new byte[1024];
|
|
string currentPosition = "";
|
public string CurrentPosition
|
{
|
get => currentPosition;
|
set
|
{
|
if (currentPosition != value)
|
{
|
currentPosition = value;
|
|
if (!string.IsNullOrWhiteSpace(currentPosition))
|
{
|
OnAGVPositoinChanged?.Invoke(this, currentPosition);
|
}
|
}
|
}
|
}
|
|
List<AGVTaskStatus> _taskRunningStates = new List<AGVTaskStatus>() { AGVTaskStatus.Running, AGVTaskStatus.Waiting, AGVTaskStatus.Suspended };
|
List<AGVTaskStatus> _taskDoneStates = new List<AGVTaskStatus>() { AGVTaskStatus.Cancelled, AGVTaskStatus.Completed, AGVTaskStatus.Failed };
|
AGVTaskStatus taskStatus = AGVTaskStatus.None;
|
public AGVTaskStatus TaskStatus
|
{
|
get => taskStatus;
|
set
|
{
|
if (taskStatus != value)
|
{
|
taskStatus = value;
|
|
if (taskStatus != AGVTaskStatus.None)
|
{
|
if (_isTaskRunning && _taskDoneStates.Contains(taskStatus))
|
{
|
_taskDoneHandle.Set();
|
_isTaskRunning = false;
|
}
|
|
OnAGVTaskStatusChanged?.Invoke(this, taskStatus);
|
}
|
}
|
}
|
}
|
|
float batteryLvl = 0;
|
public float BatteryLvl
|
{
|
get => batteryLvl;
|
set
|
{
|
if (batteryLvl != value)
|
{
|
float pre = batteryLvl;
|
batteryLvl = value;
|
OnAGVBatteryLvlChanged?.Invoke(this, pre, batteryLvl);
|
}
|
}
|
}
|
|
public string LastStation { get; set; }
|
public string Destination { get; set; } = "";
|
|
SeerMessage msg_Position = new SeerMessage();
|
SeerMessage msg_GuideStatus = new SeerMessage();
|
SeerMessage msg_Battery = new SeerMessage();
|
SeerMessage msg_IO = new SeerMessage();
|
|
public ManualResetEvent StateHandle { get; set; } = new ManualResetEvent(false);
|
public ManualResetEvent PositionHandle { get; set; } = new ManualResetEvent(false);
|
public ManualResetEvent BatteryHandle { get; set; } = new ManualResetEvent(false);
|
public ManualResetEvent IOHandle { get; set; } = new ManualResetEvent(false);
|
|
private void MonitorAGV()
|
{
|
while (CurrentState != EnumHelper.DeviceState.DSClose && CurrentState != EnumHelper.DeviceState.DSExcept)
|
{
|
_monitorLock.WaitOne();
|
try
|
{
|
SendMsg(client_State, IConfig.StatusPort, msg_Position);
|
Thread.Sleep(IConfig.ScanInterval);
|
SendMsg(client_State, IConfig.StatusPort, msg_GuideStatus);
|
Thread.Sleep(IConfig.ScanInterval);
|
SendMsg(client_State, IConfig.StatusPort, msg_Battery);
|
Thread.Sleep(IConfig.ScanInterval);
|
SendMsg(client_State, IConfig.StatusPort, msg_IO);
|
Thread.Sleep(IConfig.ScanInterval);
|
}
|
catch (Exception ex)
|
{
|
LogAsync(DateTime.Now, "监听异常", ex.GetExceptionMessage());
|
}
|
}
|
}
|
|
private void SendMsg(TcpClient client, int port, SeerMessage msg)
|
{
|
int repeatTime = 5;
|
|
do
|
{
|
try
|
{
|
InitialTcpClient(ref client, port);
|
|
var stream = client.GetStream();
|
stream.Write(msg.Frame, 0, msg.Frame.Length);
|
stream.Flush();
|
int recSize = stream.Read(buffer, 0, buffer.Length);
|
if (recSize > 0)
|
{
|
byte[] rec = buffer.Take(recSize).ToArray();
|
SeerMessage recMsg = SeerMessage.GetSeerMessage(rec);
|
if (recMsg.TypeCode != (10000 + msg.TypeCode) || recMsg.SeqNum != msg.SeqNum)
|
{
|
throw new ProcessException("反馈信息和发送信息不一致");
|
}
|
|
if (recMsg.JValues.ContainsKey("ret_code"))
|
{
|
int retCode = recMsg.JValues.Value<int>("ret_code");
|
if (retCode != 0)
|
{
|
throw new ProcessException($"反馈信息异常,异常码:{retCode}");
|
}
|
}
|
|
AnalyzeSeerMsg(recMsg);
|
}
|
|
repeatTime = 0;
|
}
|
catch (SocketException ex)
|
{
|
repeatTime--;
|
new ProcessException("AGV网络通信异常", ExceptionLevel.Warning, ex);
|
|
if (repeatTime <= 0)
|
{
|
throw new ProcessException("AGV网络通信失败", ExceptionLevel.Warning, ex);
|
}
|
|
Thread.Sleep(IConfig.ScanInterval);
|
}
|
catch (Exception ex)
|
{
|
throw new ProcessException("AGV信息发送异常", ExceptionLevel.Warning, ex);
|
}
|
|
} while (repeatTime > 0);
|
}
|
|
private async void AnalyzeSeerMsg(SeerMessage recMsg)
|
{
|
await Task.Run(() =>
|
{
|
switch (recMsg.TypeCode - 10000)
|
{
|
case (int)AGVCode.QueryPosition:
|
CurrentPosition = recMsg.JValues.Value<string>("current_station");
|
LastStation = recMsg.JValues.Value<string>("last_station");
|
|
PositionHandle.Set();
|
break;
|
case (int)AGVCode.QueryTaskStatus:
|
TaskStatus = (AGVTaskStatus)recMsg.JValues.Value<int>("task_status");
|
|
StateHandle.Set();
|
break;
|
case (int)AGVCode.QueryBattery:
|
BatteryLvl = recMsg.JValues.Value<float>("battery_level");
|
|
BatteryHandle.Set();
|
break;
|
case (int)AGVCode.QueryIO:
|
//recMsg.JValues.Value<JArray>("DI").ToList().ForEach(j =>
|
//{
|
// var ioDefinition = IConfig.IOCollection.FirstOrDefault(i => i.IOIndex == j.Value<int>("id"));
|
|
// if (ioDefinition != null && j.Value<bool>("status") != ioDefinition.CurrentValue)
|
// {
|
// ioDefinition.CurrentValue = j.Value<bool>("status");
|
// OnAGVIOChanged?.Invoke(this, ioDefinition);
|
// }
|
//});
|
|
//高电平 1 低电平 0
|
_ioDict = recMsg.JValues.Value<JArray>("DI").ToDictionary(u => u.Value<int>("id"), u => u.Value<bool>("status") ? 1 : 0);
|
IOHandle.Set();
|
break;
|
default:
|
break;
|
}
|
});
|
}
|
|
public void CancelTask()
|
{
|
SeerMessage msg = new SeerMessage((int)AGVCode.CancelTask, SID);
|
SendMsg(client_Guide, IConfig.GuidePort, msg);
|
}
|
|
public void PauseTask()
|
{
|
SeerMessage msg = new SeerMessage((int)AGVCode.PauseTask, SID);
|
SendMsg(client_Guide, IConfig.GuidePort, msg);
|
}
|
|
ManualResetEvent _taskDoneHandle = new ManualResetEvent(false);
|
ManualResetEvent _monitorLock = new ManualResetEvent(true);
|
bool _isTaskRunning = false;
|
public void TaskOrder(string dest, bool isWaitFinished)
|
{
|
_monitorLock.Reset();
|
Thread.Sleep(IConfig.ScanInterval);
|
|
CurrentPosition = "";
|
Destination = dest;
|
TaskStatus = AGVTaskStatus.None;
|
|
SeerMessage msg = new SeerMessage((int)AGVCode.TaskOrder, SID, JsonConvert.SerializeObject(new { id = dest }));
|
//OnLog?.BeginInvoke(DateTime.Now, this, $"{Name}行驶向 {dest}", null, null);
|
LogAsync(DateTime.Now, "", $"{Name}行驶向 {dest}");
|
|
SendMsg(client_Guide, IConfig.GuidePort, msg);
|
//Thread.Sleep(IConfig.ScanInterval);
|
_monitorLock.Set();
|
|
if (isWaitFinished)
|
{
|
_isTaskRunning = true;
|
_taskDoneHandle.Reset();
|
bool isNotTimeout = _taskDoneHandle.WaitOne((int)(IConfig.OperationTimeout * 60 * 1000));
|
if (!isNotTimeout)
|
{
|
throw new ProcessException($"{Name}执行去往{dest}动作超时");
|
}
|
|
if (TaskStatus == AGVTaskStatus.Failed)
|
{
|
throw new ProcessException($"{Name}执行去往{dest}动作失败");
|
}
|
else if (TaskStatus == AGVTaskStatus.Cancelled)
|
{
|
//OnLog?.Invoke(DateTime.Now, this, $"{Name}执行去往{dest}动作取消");
|
LogAsync(DateTime.Now, "", $"{Name}执行去往{dest}动作取消");
|
}
|
}
|
}
|
|
#region IMonitor
|
public event OnMonitorInvokeDelegate OnMonitorInvoke;
|
public event OnMonitorAlarmDelegate OnMonitorAlarm;
|
|
Dictionary<int, int> _ioDict = new Dictionary<int, int>();
|
public void Monitor()
|
{
|
while (CurrentState != EnumHelper.DeviceState.DSClose && CurrentState != EnumHelper.DeviceState.DSExcept)
|
{
|
try
|
{
|
IOHandle.Reset();
|
|
var isNotTimeout = IOHandle.WaitOne(IConfig.ScanInterval * 3);
|
|
if (!isNotTimeout)
|
throw new ProcessException($"{Name}监听超时");
|
|
IConfig.MonitorSetCollection.ForEach(m =>
|
{
|
if (_ioDict.ContainsKey(m.TriggerIndex))
|
{
|
if ((m.TriggerValue == _ioDict[m.TriggerIndex] || m.TriggerIndex == -999) && _ioDict[m.TriggerIndex] != m.CurrentValue)
|
{
|
if (m.OpConfig == null)
|
{
|
m.OpConfig = new OperationConfigBase();
|
}
|
|
m.OpConfig.InputPara = m.InputDataIndex.ConvertAll(index =>
|
{
|
if (_ioDict.ContainsKey(index))
|
{
|
return _ioDict[index];
|
}
|
else
|
{
|
return -1;
|
}
|
}).ToList();
|
|
OnMonitorInvoke?.BeginInvoke(DateTime.Now, this, m, null, null);
|
}
|
|
m.CurrentValue = _ioDict[m.TriggerIndex];
|
}
|
});
|
}
|
catch (Exception ex)
|
{
|
OnLog?.Invoke(DateTime.Now, this, $"{Name}监听异常:{ex.GetExceptionMessage()}");
|
}
|
}
|
}
|
|
public virtual void ResetAlarm()
|
{
|
IConfig.WarningSetCollection.ForEach(u => u.CurrentStatus = !u.TriggerValue);
|
}
|
#endregion
|
}
|
}
|
