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}"); } }