using Bro.Common.Base;
|
using Bro.Common.Helper;
|
using Bro.Common.Interface;
|
using Newtonsoft.Json;
|
using System;
|
using System.Collections.Generic;
|
using System.Linq;
|
using System.Net;
|
using System.Net.Sockets;
|
using System.Runtime.InteropServices;
|
using System.Text;
|
using System.Threading;
|
using System.Threading.Tasks;
|
|
namespace Bro.Device.SeerAGV
|
{
|
[Device("SeerAGV", "SeerAGV", EnumHelper.DeviceAttributeType.Device)]
|
public class SeerAGVDriver : DeviceBase
|
{
|
public Action<SeerAGVDriver, string> OnAGVPositoinChanged;
|
public Action<SeerAGVDriver, AGVTaskStatus> OnAGVTaskStatusChanged;
|
public Action<SeerAGVDriver, float> OnAGVBatteryLvlChanged;
|
|
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);
|
}
|
|
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 }) : "");
|
|
Task.Run(() =>
|
{
|
MonitorAGV();
|
});
|
}
|
|
protected override void Stop()
|
{
|
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);
|
}
|
}
|
}
|
}
|
|
AGVTaskStatus taskStatus = AGVTaskStatus.None;
|
public AGVTaskStatus TaskStatus
|
{
|
get => taskStatus;
|
set
|
{
|
if (taskStatus != value)
|
{
|
taskStatus = value;
|
|
if (taskStatus != AGVTaskStatus.None)
|
{
|
OnAGVTaskStatusChanged?.Invoke(this, taskStatus);
|
}
|
}
|
}
|
}
|
|
float batteryLvl = 0;
|
public float BatteryLvl
|
{
|
get => batteryLvl;
|
set
|
{
|
if (batteryLvl != value)
|
{
|
batteryLvl = value;
|
OnAGVBatteryLvlChanged?.Invoke(this, batteryLvl);
|
}
|
}
|
}
|
|
SeerMessage msg_Position = new SeerMessage();
|
SeerMessage msg_GuideStatus = new SeerMessage();
|
SeerMessage msg_Battery = new SeerMessage();
|
private void MonitorAGV()
|
{
|
while (CurrentState != EnumHelper.DeviceState.DSClose && CurrentState != EnumHelper.DeviceState.DSExcept)
|
{
|
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);
|
}
|
catch (Exception ex)
|
{
|
OnLog?.Invoke(DateTime.Now, this, $"{Name}监听异常：{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);
|
|
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("反馈信息和发送信息不一致", null);
|
}
|
|
if (recMsg.JValues.ContainsKey("ret_code"))
|
{
|
int retCode = recMsg.JValues.Value<int>("ret_code");
|
if (retCode != 0)
|
{
|
throw new ProcessException($"反馈信息异常,异常码:{retCode}", null);
|
}
|
}
|
|
AnalyzeSeerMsg(recMsg);
|
}
|
|
repeatTime = 0;
|
}
|
catch (SocketException ex)
|
{
|
repeatTime--;
|
new ProcessException("AGV网络通信异常", ex);
|
|
if (repeatTime <= 0)
|
{
|
throw new ProcessException("AGV网络通信失败", ex);
|
}
|
|
Thread.Sleep(IConfig.ScanInterval);
|
}
|
catch (Exception ex)
|
{
|
throw new ProcessException("AGV信息发送异常", 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");
|
break;
|
case (int)AGVCode.QueryTaskStatus:
|
TaskStatus = (AGVTaskStatus)recMsg.JValues.Value<int>("task_status");
|
break;
|
case (int)AGVCode.QueryBattery:
|
BatteryLvl = recMsg.JValues.Value<float>("battery_level");
|
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);
|
}
|
|
public void TaskOrder(string dest)
|
{
|
CurrentPosition = "";
|
SeerMessage msg = new SeerMessage((int)AGVCode.TaskOrder, SID, JsonConvert.SerializeObject(new { id = dest }));
|
SendMsg(client_Guide, IConfig.GuidePort, msg);
|
}
|
}
|
}
|
