public RobotInfoTestHandler(WebSocketObjectHolder webSocketObjectHolder) : base(webSocketObjectHolder)
{
DateTime s = DateTime.Now;
this.testLoop = Task.Run(async() => {
this.isStop = false;
var random = new Random();
var seed = 2.0;
while (!this.isStop)
{
var randomFArray = Enumerable.Range(0, 6).Select(i => Convert.ToSingle(seed * random.NextDouble() - seed / 2.0)).ToArray();
var data = new Position
{
X = randomFArray[0],
Y = randomFArray[1],
Z = randomFArray[2],
Role = randomFArray[3],
Pitch = randomFArray[4],
Yaw = randomFArray[5],
};
var res = new RobotInfo
{
Tcp = data,
};
var resStr = JsonConvert.SerializeObject(res);
await SendMessageToAllAsync(resStr, AppConst.NON_BOM_UTF8_ENCORDING);
System.Threading.Thread.Sleep(10);
}
});
}
private void CfdLogger_RobotInfoUpdated(RobotInfo info)
{
this.CurrentRobotInfo = info;
}
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;
}
}
