public void AddStatusMessage(AGVComFrame msg)
{
lock (lockerStatus)
{
msgStatusList.Enqueue(msg);
}
}
public void AddTaskMessage(AGVComFrame msg)
{
lock (lockerTask)
{
msgTaskList.Enqueue(msg);
}
}
public void AddMessage(AGVComFrame msg)
{
lock (locker)
{
msgList.Enqueue(msg);
}
}
/// <summary>
/// 定时获取AGV导航状态
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void timerStatus_Elapsed(object sender, ElapsedEventArgs e)
{
lock (lockerStatus)
{
if (msgStatusList.Count > 0)
{
AGVComFrame response = comStatus.SendAndGet(msgStatusList.Dequeue(), 300);
if (null != response && null != response.data && null != this.OnStatusUpdate)
{
string data = response.Message;
AGVTypes _type = (AGVTypes)(response.header.type - AGVProtocolHeader.TypeResponseOffset);
string respData = _type.ToString() + ":" + data;
this.OnStatusUpdate.BeginInvoke(this, respData, null, null);
}
}
//AGVComFrame response = comStatus.SendAndGet(AGVComFrameBuilder.状态_查询机器人导航状态(), 300);
//this.status = AGVComFrameBuilder.Response_状态_查询机器人导航状态(ref response);
//if (null != this.status && null != this.OnStatusUpdate)
//{
// string data = System.Text.ASCIIEncoding.UTF8.GetString(response.data);
// this.OnStatusUpdate.BeginInvoke(this, data, null, null);
//}
}
}
public AGVComFrame Send(AGVComFrame msg)
{
AGVComFrame frame = null;
//导航
if (msg.header.type >= 3001 && msg.header.type < 3106)
{
frame = comTask.SendAndGet(msg, 200);
}
return(frame);
}
public AGVStatusPositionFrame GetPosition()
{
AGVComFrame recvFrame = comStatus.SendAndGet(new AGVComFrame(AGVFrameTypes.状态_查询机器人位置, null));
if (recvFrame != null)
{
AGVStatusPositionFrame resp = recvFrame.DataParse <AGVStatusPositionFrame>();
if (null != resp && resp.RetCode == AGVErrorCodeTypes.成功)
{//导航获取成功
return(resp);
}
}
return(null);
}
/// <summary>
/// 取消导航
/// </summary>
public AGVErrorCodeTypes CancelNavi()
{
AGVComFrame sendFrame = new AGVComFrame(AGVFrameTypes.导航_取消当前导航, null);
AGVComFrame recvFrame = comNavi.SendAndGet(sendFrame);
if (recvFrame != null)
{
AGVNavigationResponse resp = recvFrame.DataParse <AGVNavigationResponse>();
if (null != resp)
{//导航取消成功
return(resp.RetCode);
}
}
return(AGVErrorCodeTypes.未知错误);
}
public AGVStatusNavigationStateFrame GetTaskStatus(bool simple = false)
{
AGVComFrame sendFrame = new AGVComFrame(AGVFrameTypes.状态_查询机器人导航状态, new { simple = simple });
AGVComFrame recvFrame = comStatus.SendAndGet(sendFrame);
if (recvFrame != null)
{
AGVStatusNavigationStateFrame resp = recvFrame.DataParse <AGVStatusNavigationStateFrame>();
if (null != resp && resp.RetCode == AGVErrorCodeTypes.成功)
{//导航获取成功
return(resp);
}
}
return(null);
}
/// <summary>
/// 获取导航路径
/// </summary>
/// <param name="target">目的地</param>
/// <returns></returns>
public List <string> GetPathNavi(string target)
{
AGVComFrame sendFrame = new AGVComFrame(AGVFrameTypes.导航_获取路径导航的路径, new { id = target });
AGVComFrame recvFrame = comNavi.SendAndGet(sendFrame);
if (recvFrame != null)
{
AGVNavigationResponse resp = recvFrame.DataParse <AGVNavigationResponse>();
if (null != resp && resp.RetCode == AGVErrorCodeTypes.成功)
{//导航获取成功
return(resp.Path);
}
}
return(null);
}
/// <summary>
/// 发送并获取反馈
/// </summary>
/// <param name="frame"></param>
/// <param name="timeout">超时时间，单位ms</param>
/// <returns></returns>
public AGVComFrame SendAndGet(AGVComFrame frame, int timeout = 300)
{
if (null != frame)
{
var msg = client.WriteLineAndGetReply(frame.Pack(), new TimeSpan(0, 0, 0, 0, timeout));
if (null == msg)
{
return(null);
}
return(AGVComFrame.Parse(msg.Data));
}
else
{
return(null);
}
}
private void timerTask_Elapsed(object sender, ElapsedEventArgs e)
{
lock (lockerTask)
{
if (msgTaskList.Count > 0)
{
AGVComFrame response = comTask.SendAndGet(msgTaskList.Dequeue(), 300);
if (null != response && null != response.data && null != this.OnTaskUpdate)
{
string data = response.Message;
AGVTypes _type = (AGVTypes)(response.header.type - AGVProtocolHeader.TypeResponseOffset);
string respData = _type.ToString() + ":" + data;
this.OnTaskUpdate.BeginInvoke(this, respData, null, null);
}
}
}
}
public static AGVComFrame Parse(byte[] buf)
{
if (buf.Length < HeadLength)
{
return(null);
}
IntPtr ptr = Marshal.AllocHGlobal(HeadLength);
AGVComFrame tmp = null;
try
{
Marshal.Copy(buf, 0, ptr, HeadLength);
SeerHead head = Marshal.PtrToStructure <SeerHead>(ptr);
head.number = (UInt16)((short)IPAddress.HostToNetworkOrder((short)head.number));
head.length = (UInt32)((int)IPAddress.HostToNetworkOrder((int)head.length));
head.type = (UInt16)((short)IPAddress.HostToNetworkOrder((short)head.type));
tmp = new AGVComFrame();
if (head.type > TYPE_RESPONSE_OFFSET)
{
tmp.IsResponseFrame = true;
head.type = (ushort)(head.type - TYPE_RESPONSE_OFFSET);
}
tmp.FrameType = (AGVFrameTypes)head.type;
uint length = head.length;
if (length <= buf.Length - HeadLength)
{
tmp.Data = new byte[length];
Array.Copy(buf, HeadLength, tmp.Data, 0, head.length);
}
else
{
tmp = null;
}
}
finally
{
Marshal.FreeHGlobal(ptr);
}
return(tmp);
}
private void button状态_Click(object sender, EventArgs e)
{
AGVTypes value = (AGVTypes)this.comboBox1.SelectedItem;
AGVComFrame frame = new AGVComFrame(new AGVProtocolHeader()
{
type = (UInt16)AGVTypes.导航_路径导航
}, "{\"id\":\"LM1\"}");
if (agv1.IsConnected)
{
//agv1.AddTaskMessage(frame);
agv1.AddStatusMessage(AGVComFrameBuilder.NoData_Request(value));
}
else
{
MessageBox.Show("设备未连接，请先连接设备！", "错误");
}
}
/// <summary>
/// 设置路径导航
/// </summary>
/// <param name="pos"></param>
public AGVErrorCodeTypes SetNavi(string pos)
{
AGVNavigationDataFrame naviData = new AGVNavigationDataFrame()
{
Id = pos,
Operation = oprations.Load,
Layer = 0,
Recognize = false
};
AGVComFrame sendFrame = new AGVComFrame(AGVFrameTypes.导航_路径导航, naviData);
AGVComFrame recvFrame = comNavi.SendAndGet(sendFrame);
if (recvFrame != null)
{
AGVNavigationResponse resp = recvFrame.DataParse <AGVNavigationResponse>();
if (null != resp)
{//导航发送成功
return(resp.RetCode);
}
}
return(AGVErrorCodeTypes.未知错误);
}