internal RTDEReceiver(NetworkStream stream, RobotModel robotModel, List <KeyValuePair <string, string> > rtdeOutputConfiguration, List <KeyValuePair <string, string> > rtdeInputConfiguration, ConcurrentQueue <RobotModel> robotData)
{
_robotModel = robotModel;
_stream = stream;
_rtdeOutputConfiguration = rtdeOutputConfiguration;
_rtdeInputConfiguration = rtdeInputConfiguration;
//_robotData = robotData;
_rtdeDataPackageDecoder = new RTDEDatePackageDecoder(_rtdeOutputConfiguration, _rtdeDataPackageQueue, robotData);
//_packageDecoderThread = new Thread(SocketPacageDecoder);
//_packageDecoderThread.Start();
_receiverThread = new Thread(Run);
_receiverThread.Start();
}
private URControlVersion DecodeUniversalRobotsControllerVersion(byte[] payload)
{
byte[] majorArray = new byte[4];
Array.Copy(payload, 0, majorArray, 0, 4);
majorArray = RTDEDatePackageDecoder.CheckEndian(majorArray);
UInt32 major = BitConverter.ToUInt32(majorArray, 0);
byte[] minorArray = new byte[4];
Array.Copy(payload, 4, minorArray, 0, 4);
minorArray = RTDEDatePackageDecoder.CheckEndian(minorArray);
UInt32 minor = BitConverter.ToUInt32(minorArray, 0);
byte[] bugfixArray = new byte[4];
Array.Copy(payload, 8, bugfixArray, 0, 4);
bugfixArray = RTDEDatePackageDecoder.CheckEndian(bugfixArray);
UInt32 bugfix = BitConverter.ToUInt32(bugfixArray, 0);
byte[] buildArray = new byte[4];
Array.Copy(payload, 12, buildArray, 0, 4);
buildArray = RTDEDatePackageDecoder.CheckEndian(buildArray);
UInt32 build = BitConverter.ToUInt32(buildArray, 0);
return(new URControlVersion((int)major, (int)minor, (int)bugfix, (int)build));
}
private void DecodeRtdePacage(byte[] recievedPackage)
{
DateTime startTime = DateTime.Now;
byte[] sizeArray = new byte[2];
UR_RTDE_Command type = (UR_RTDE_Command)recievedPackage[2];
Array.Copy(recievedPackage, sizeArray, 2);
sizeArray = RTDEDatePackageDecoder.CheckEndian(sizeArray);
ushort size = BitConverter.ToUInt16(sizeArray, 0);
if ((size > 3) && (size == recievedPackage.Length))
{
byte[] payloadArray = new byte[size - 3];
Array.Copy(recievedPackage, 3, payloadArray, 0, size - 3);
switch (type)
{
case UR_RTDE_Command.RTDE_REQUEST_PROTOCOL_VERSION:
_robotModel.RTDEProtocolVersion = DecodeProtocolVersion(payloadArray);
break;
case UR_RTDE_Command.RTDE_GET_URCONTROL_VERSION:
_robotModel.URControlVersion = DecodeUniversalRobotsControllerVersion(payloadArray);
break;
case UR_RTDE_Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
DecodeRTDESetupPackage(payloadArray, _rtdeOutputConfiguration);
break;
case UR_RTDE_Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
DecodeRTDESetupPackage(payloadArray, _rtdeInputConfiguration);
break;
case UR_RTDE_Command.RTDE_DATA_PACKAGE:
if (!readFixedRTDEPackageSize)
{
readFixedRTDEPackageSize = true;
}
_rtdeDataPackageQueue.Enqueue(payloadArray);
var delta = DateTime.Now - lastRtdeDataPackTime;
if (delta.TotalMilliseconds > 100)
{
Console.WriteLine($"Real time since last RTDE Package decode: {delta.TotalMilliseconds} milliseconds");
}
lastRtdeDataPackTime = DateTime.Now;
break;
case UR_RTDE_Command.RTDE_CONTROL_PACKAGE_START:
_robotModel.RTDEConnectionState = DecodeRTDEControlPackageStart(payloadArray);
break;
case UR_RTDE_Command.RTDE_CONTROL_PACKAGE_PAUSE:
_robotModel.RTDEConnectionState = DecodeRTDEControlPacagePause(payloadArray);
break;
default:
log.Error("Package type not implemented " + type);
throw new NotImplementedException("Package type not implemented " + type);
}
}
else
{
log.Error("Got a packet of unexpected size");
readFixedRTDEPackageSize = false;
}
var decodeRtdePackageTime = DateTime.Now - startTime;
if (decodeRtdePackageTime.TotalMilliseconds > 2)
{
Console.WriteLine($"Time to Decode a RTDE package: {decodeRtdePackageTime.TotalMilliseconds}");
}
}