private byte[] createMessage(flexData data)
{
//XmlSerializerオブジェクトを作成
//オブジェクトの型を指定する
byte[] buf = null;
using (System.IO.MemoryStream ms = new System.IO.MemoryStream())
{
var writer = System.Xml.XmlWriter.Create(ms, new System.Xml.XmlWriterSettings()
{
Encoding = Encoding.ASCII,
Indent = false,
OmitXmlDeclaration = false,
});
serializer.Serialize(writer, data);
ms.Position = 0;
buf = ms.GetBuffer();
}
byte[] messageBuf = new byte[buf.Length + 8];
Array.Copy(BitConverter.GetBytes(MAGIC_NUMBER), 0, messageBuf, 0, 2);
Array.Copy(BitConverter.GetBytes(VERSION_NUMBER), 0, messageBuf, 2, 2);
Array.Copy(BitConverter.GetBytes(buf.Length), 0, messageBuf, 4, 4);
Array.Copy(buf, 0, messageBuf, 8, buf.Length);
return(messageBuf);
}
public CFD_Logger()
{
var flex = new flexData
{
version = "1",
dataExchange = new type_dataExchange
{
dataRequest = new type_dataExchange_dataRequest[]
{
new type_dataExchange_dataRequest
{
data = new type_dataExchange_dataRequest_data[]
{
new type_dataExchange_dataRequest_data
{
unit = "1",
group = type_DataGroup.Generic,
id = "SYSTEM!",
subid = "810",
count = "6",
push = "-1",
threshold = 0.01,
priority = "-1"
}
}
}
}
}
};
this.sendMessage = createMessage(flex);
}
public RobotInfoHandler(WebSocketObjectHolder webSocketObjectHolder, Settings settings) : base(webSocketObjectHolder)
{
this.settings = settings;
this.robotInfo = new RobotInfo
{
Tcp = new Position
{
X = 0,
Y = 0,
Z = 0,
Role = 0,
Pitch = 0,
Yaw = 0
}
};
var flex = new flexData
{
version = "1",
dataExchange = new type_dataExchange
{
dataRequest = new type_dataExchange_dataRequest[]
{
new type_dataExchange_dataRequest
{
data = new type_dataExchange_dataRequest_data[]
{
new type_dataExchange_dataRequest_data
{
unit = "1",
group = type_DataGroup.Generic,
id = "SYSTEM!",
subid = "810",
count = "6",
push = "-1",
threshold = 0.01,
priority = "5"
}
}
}
}
}
};
this.sendMessage = createMessage(flex);
}
public override async Task OnConnected(WebSocket client)
{
await base.OnConnected(client);
var socketid = WebSocketObjectHolder.GetId(client);
Console.WriteLine($"socket created: {socketid}");
if (this.watchLoop == null)
{
this.watchLoop = Task.Run(async() =>
{
Connect();
isStop = false;
while (!isStop)
{
this.socket.Send(this.sendMessage);
int receiveLength = socket.Receive(this.resBuffer);
flexData resData = null;
using (var ms = new System.IO.MemoryStream())
{
try
{
var length = ParseHeader(this.resBuffer, 0);
System.Diagnostics.Debug.WriteLine(length);
// ms.Write(res, 8, receiveLength - 8);
ms.Write(this.resBuffer, 8, length);
ms.Position = 0;
//Console.WriteLine(Encoding.UTF8.GetString(ms.GetBuffer()));
resData = (flexData)serializer.Deserialize(ms);
if (resData.notifications != null)
{
resData.notifications.ToList().ForEach(n =>
{
Console.Error.WriteLine($"{n.code}: {n.message}");
});
Thread.Sleep(1000);
continue;
}
var updateData = resData.dataExchange.dataUpdate[0].data[0].r;
await infoSemaphore.WaitAsync();
try
{
this.robotInfo.Tcp.X = (float)updateData[0];
this.robotInfo.Tcp.Y = (float)updateData[1];
this.robotInfo.Tcp.Z = (float)updateData[2];
this.robotInfo.Tcp.Role = (float)updateData[3];
this.robotInfo.Tcp.Pitch = (float)updateData[4];
this.robotInfo.Tcp.Yaw = (float)updateData[5];
}
finally
{
infoSemaphore.Release();
}
Thread.Sleep(5);
}
catch (System.Exception err)
{
Console.WriteLine(err.ToString());
Thread.Sleep(5000);
throw;
}
finally
{
ms.Dispose();
}
}
}
});
}
Thread.Sleep(100);
if (sendLoop == null)
{
this.sendLoop = Task.Run(async() =>
{
isStop = false;
string str = null;
while (!isStop)
{
await infoSemaphore.WaitAsync();
try
{
str = JsonConvert.SerializeObject(this.robotInfo);
}
finally
{
infoSemaphore.Release();
}
//Console.WriteLine(str);
await SendMessageToAllAsync(str, AppConst.NON_BOM_UTF8_ENCORDING);
Thread.Sleep(10);
}
});
}
await Task.CompletedTask;
}
public void Connect(string ip, int port)
{
if (socket != null)
{
DisConnect();
}
this.socket = new Socket(SocketType.Stream, ProtocolType.Tcp);
this.socket.Connect(new IPEndPoint(IPAddress.Parse(ip), port));
if (this.watchLoop == null)
{
this.watchLoop = Task.Run(() =>
{
isStop = false;
while (!isStop)
{
this.socket.Send(this.sendMessage);
int receiveLength = socket.Receive(this.resBuffer);
flexData resData = null;
using (var ms = new System.IO.MemoryStream())
{
try
{
var length = ParseHeader(this.resBuffer, 0);
//System.Diagnostics.Debug.WriteLine(length);
ms.Write(this.resBuffer, 8, length);
ms.Position = 0;
if (length == 0)
{
continue;
}
resData = (flexData)serializer.Deserialize(ms);
if (resData.notifications != null)
{
//resData.notifications.ToList().ForEach(n =>
//{
// Console.Error.WriteLine($"{n.code}: {n.message}");
//});
//Thread.Sleep(1000);
continue;
}
var updateData = resData.dataExchange.dataUpdate[0].data[0].r;
var info = new RobotInfo
{
Tcp = new Position
{
X = (float)updateData[0],
Y = (float)updateData[1],
Z = (float)updateData[2],
Role = (float)updateData[3],
Pitch = (float)updateData[4],
Yaw = (float)updateData[5],
},
Time = DateTime.Now.ToFileTime(),
};
this.RobotInfoUpdated?.Invoke(info);
Thread.Sleep(1);
}
catch (System.Exception err)
{
Console.WriteLine(err.ToString());
Thread.Sleep(5000);
return;
}
finally
{
ms.Dispose();
}
}
}
});
IsLogging = true;
}
}
