Commit Iniziale

2019-10-21 11:37:31 +02:00
commit 44ac6e8802
56 changed files with 32022 additions and 0 deletions
--- a/libURcom/URCLinterface.cpp
+++ b/libURcom/URCLinterface.cpp
@@ -0,0 +1,258 @@
+/*
+ * URCLinterface.cpp
+ *
+ *  Created on: Jan 14, 2019
+ *      Author: emanuele
+ *
+ *  Implementazione C++ del protocollo di comunicazione socket per Universal Robot e-series.
+ *  Protocollo client interface, comunicazione 10Hz su porte 30001 - 30002, informazioni e messaggi parziali,
+ *  invio di comandi URscript
+ *
+ *  20190114 - Prima Scittura
+ *  20190114 - Inizio implementazione della connessione socket con libreria boost, scritta funzione connect.
+ *  20190115 - Continuo scirttura funzione dataRead(), verificato il protocollo rispetto al manuale, la funzione legge e divide corretamente i pacchetti
+ *  			tra di loro. ora bisogna separare i sottopacchetti.
+ *  20190116 - La separazione dei sottopacchetti viene fatta da delle classi specializzate che scompattano e cambiano
+ *  			l'endianness dei dati se necessario.
+ *  20190117 - Prima scirttura delle classi che scompattano i dati, scirtta versionMessage, risolti alcuni dubbi
+ *  			e problemi con i puntatori. A quanto pare non è disponibile una libreria che faccia quello che
+ *  			fa struct.unpack di python
+ *  20190118 - Completata la scrittura delle classi che scompattano i dati, le strutture combaciano con il
+ *  			manuale. Inizio della scrittura finzioni che mandano i comandi URscript al controller,
+ *  			valutare se vale la pena implementare per ogn tipo di funzione movimento
+ *  			prevista in URscript o no..
+ *  20190121 - Passaggio a nuova Vm con Ubuntu 18.04LTS e g++ 7.3.0, risolti alcuni problemi di compilazione,
+ *  			passaggio a librerie boost-1.68
+ *  			Scritta funzione di invio script grezzi al controller, inizio scrittura a thread che continua a ricevere
+ *  			i dati dal robot.
+ */
+
+#include "URCLinterface.h"
+
+using namespace subpackage;
+using namespace std;
+using boost::format;
+using std::string;
+
+
+UR_CLinterface::UR_CLinterface(shared_ptr<spdlog::logger> logger) {
+	log = logger;
+	log->info("Initializing UR Client Interface"); // @suppress("Ambiguous problem")
+	rsocket = new tcp::socket(io_service);
+}
+
+UR_CLinterface::~UR_CLinterface() {
+	// TODO Auto-generated destructor stub
+	if (rsocket->is_open()) this->disconnect();
+	delete rsocket;
+}
+
+bool UR_CLinterface::connect(const char* ip, uint16_t port) {
+	try {
+		rsocket->connect(
+				tcp::endpoint(boost::asio::ip::address::from_string(ip), port));
+		if ((bool) rsocket->is_open()) {
+			log->info("Connected to {0} on port {1}", ip, port);
+			return true;
+		}
+	} catch (boost::system::error_code* e) {
+		log->error(e->message());
+		return false;
+	}
+	return false;
+}
+
+void UR_CLinterface::disconnect(void) {
+	rsocket->close(error);
+	if (error){
+		log->error("Cannot close socket, code: {}", error.message()); // @suppress("Assignment in condition")
+		return;
+	}
+	log->info("Disconnected"); // @suppress("Ambiguous problem")
+}
+
+void UR_CLinterface::sendCommand(std::string command) {
+	char* dataOut=NULL;
+	if (rsocket->is_open()){
+		command = command + '\n';
+		dataOut = (char*)malloc(command.size());
+		strncpy(dataOut, command.c_str(), command.size());
+		log->debug("Sending: {}, s:{}",dataOut,command.size());
+		boost::asio::write(*rsocket, boost::asio::buffer(dataOut, command.size()), error);
+		free(dataOut);
+	}
+	if (!error) return;
+	else {
+		log->error("Failed to send data to socket, code: {}", error.message());
+	}
+
+}
+
+void UR_CLinterface::parseData(void){
+	uint32_t packageSize=lastDataLen;
+	uint32_t subPackageSize;
+	unsigned char packageType;
+	unsigned char subPackageType;
+	uint32_t currIndex=0;
+	clientSubpackage* subP=NULL;
+
+	//main message type is on 4th byte of the message
+	packageType = lastData[sizeof(uint32_t)];
+
+	switch(packageType){
+		case VERSION_MESSAGE: //begin version message parse
+			if (firstMessage){
+				log->info("Version Message: size[{}]",packageSize); // @suppress("Ambiguous problem")
+				subP = new versionSub(lastData, &clientData);
+				subP->parse();
+				firstMessage=false;
+				long t=(long)clientData.versonMessage.timestamp;
+                std::string timestr=std::string(asctime(gmtime(&t)));
+                timestr.pop_back();
+                log->info("Connection Time: {}", timestr);
+				log->info("Controller Version {0}.{1}", clientData.versonMessage.majorVersion,
+						clientData.versonMessage.minorVersion);
+				log->info("ProjectName: {}", clientData.versonMessage.projectName->c_str());
+			}
+			break;
+
+		case ROBOT_MESSAGE: // begin subpacket parse
+			// move index to skip package leader
+			currIndex = sizeof(uint32_t)+sizeof(char);
+			log->debug("Robot Message: size[{}]",packageSize);
+			// iterate until end of package
+			while (currIndex < lastDataLen){
+				// free memory
+				if (subP != NULL) {
+					delete subP;
+					subP = NULL;
+				}
+
+				//extract subpackage size
+				memcpy(&subPackageSize, lastData+currIndex, sizeof(uint32_t));
+				subPackageSize=big_to_native(subPackageSize);
+				// extract subpackage type
+				subPackageType=lastData[currIndex+sizeof(uint32_t)];
+
+				try{
+					switch (subPackageType){
+						case ROBOT_MODE_DATA:
+							log->debug("SubP_ModeData"); // @suppress("Ambiguous problem")
+							subP = new robotModeSub(&lastData[currIndex], &clientData);
+							break;
+						case JOINT_DATA:
+							log->debug("SubP_JointData"); // @suppress("Ambiguous problem")
+							subP = new jointDataSub(&lastData[currIndex], &clientData);
+							break;
+						case TOOL_DATA:
+							log->debug("SubP_ToolData"); // @suppress("Ambiguous problem")
+							subP = new toolDataSub(&lastData[currIndex], &clientData);
+							break;
+						case MASTER_BOARD_DATA:
+							log->debug("SubP_MasterBoardData"); // @suppress("Ambiguous problem")
+							subP = new masterBoardSub(&lastData[currIndex], &clientData);
+							break;
+						case CARTESIAN_INFO:
+							log->debug("SubP_CartesianInfo"); // @suppress("Ambiguous problem")
+							subP = new cartesianInfoSub(&lastData[currIndex], &clientData);
+							break;
+						case KINEMATICS_INFO:
+							log->debug("SubP_KinematicsInfo"); // @suppress("Ambiguous problem")
+							subP = new kineInfoSub(&lastData[currIndex], &clientData);
+							break;
+						case CONFIGURATION_DATA:
+							log->debug("SubP_ConfigurationData"); // @suppress("Ambiguous problem")
+							subP = new configurationSub(&lastData[currIndex], &clientData);
+							break;
+						case FORCE_MODE_DATA:
+							log->debug("SubP_ForceModeData"); // @suppress("Ambiguous problem")
+							subP = new forceModeSub(&lastData[currIndex], &clientData);
+							break;
+						case ADDITIONAL_INFO:
+							log->debug("SubP_AdditionalInfo"); // @suppress("Ambiguous problem")
+							subP = new additionalInfoSub(&lastData[currIndex], &clientData);
+							break;
+						case CALIBRATION_DATA:
+							log->debug("SubP_CalibrationData"); // @suppress("Ambiguous problem")
+							subP = new calibrationSub(&lastData[currIndex], &clientData);
+							break;
+						case TOOL_COM_INFO:
+							log->debug("SubP_ToolComInfo"); // @suppress("Ambiguous problem")
+							subP = new toolComSub(&lastData[currIndex], &clientData);
+							break;
+						case TOOL_MODE_INFO:
+							log->debug("SubP_ToolModeInfo"); // @suppress("Ambiguous problem")
+							subP = new toolModeSub(&lastData[currIndex], &clientData);
+							break;
+						case SAFETY_DATA:
+							//not yet implemented
+							break;
+						default:
+							log->error("Unknown Subpackage: {}", subPackageType);
+							break;
+					}
+					if (subP != NULL) subP->parse();
+					currIndex+=subPackageSize;
+				} catch (long sizediff){
+					log->error("Package Length Mismatch: {}",sizediff);
+					currIndex+=subPackageSize;
+				}
+			}
+			log->debug("End of Package"); // @suppress("Ambiguous problem")
+			break;
+
+		default:
+			log->error("Unknown package type: {}",packageType);
+			break;
+	}
+	clientData.lastUpdate = time(NULL);
+}
+
+void UR_CLinterface::readData(void) {
+	uint32_t packlen;
+	uint32_t readata = 0;
+	string rawdata;
+
+	if (rsocket->is_open()){
+		// clear last data vector
+		if(lastData!=NULL)
+			free(lastData);
+		//read first incoming byte for packet length
+		boost::asio::read(*rsocket, receive_buffer, boost::asio::transfer_exactly(HEADERSIZE), error);
+		if (!error) {
+			const unsigned char* f = boost::asio::buffer_cast<const unsigned char*>(receive_buffer.data());
+			memcpy(&packlen, f, HEADERSIZE * sizeof(char));
+			// all number are represented in network byte order, conversion required
+			packlen = ntohl(packlen);
+			// allocate memory for incoming package
+			lastData = (unsigned char*)malloc(packlen);
+			// blocking read of exactly packlen bytes
+			readata += boost::asio::read(*rsocket, receive_buffer,boost::asio::transfer_exactly(packlen), error);
+			//retreive all data from buffer
+			const unsigned char* ff = boost::asio::buffer_cast<const unsigned char*>(receive_buffer.data());
+			// flush buffer for reuse
+			receive_buffer.consume(packlen);
+
+			// print packet dump
+			log->debug("Successful read: {0} bytes", readata);
+			if (log->level() == spdlog::level::trace) {
+				char hh[5];
+				for (uint32_t k = 0; k < packlen; k++) {
+					sprintf(hh, "%02x|", ff[k]);
+					rawdata.append(hh);
+					if (k % 16 == 0 && k != 0)
+						rawdata.append("\n");
+				}
+				log->trace("\n{}\n", rawdata);
+			}
+			//export data to vector
+			memcpy(lastData, ff, packlen);
+			lastDataLen=packlen;
+		} else {
+			log->error("Failed to read data from socket, code: {}", error.message());
+		}
+	} else {
+		log->error("Robot not connected!, call connect(ip,port) instead"); // @suppress("Ambiguous problem")
+	}
+}
+
--- a/libURcom/URCLinterface.h
+++ b/libURcom/URCLinterface.h
@@ -0,0 +1,54 @@
+/*
+ * URCLinterface.h
+ *
+ *  Created on: Jan 14, 2019
+ *      Author: emanuele
+ */
+
+#ifndef URCLINTERFACE_H_
+#define URCLINTERFACE_H_
+
+#include "clientMessage.h"
+#include <ctime>
+#include <iostream>
+#include <arpa/inet.h>
+#include <spdlog/spdlog.h>
+#include <boost/asio.hpp>
+#include <boost/format.hpp>
+
+using namespace std;
+using namespace boost::asio;
+using ip::tcp;
+
+
+class UR_CLinterface {
+public:
+	UR_CLinterface(shared_ptr<spdlog::logger>);
+	virtual ~UR_CLinterface();
+
+	bool connect(const char* ip, uint16_t port);
+	void disconnect();
+
+	void sendCommand(std::string command);
+	void sendPose(pose_t pos);
+	void sendJointPose(jointPose_t pos);
+
+	void readData();
+	void parseData();
+
+	clientData_t clientData;
+private:
+	shared_ptr<spdlog::logger> log;
+
+	boost::asio::io_service io_service;
+	boost::asio::streambuf receive_buffer;
+	boost::system::error_code error;
+	tcp::socket* rsocket;
+
+	uint32_t lastDataLen=0;
+	unsigned char* lastData=NULL;
+	bool firstMessage=true;
+
+};
+
+#endif /* URCLINTERFACE_H_ */
--- a/libURcom/clientMessage.cpp
+++ b/libURcom/clientMessage.cpp
@@ -0,0 +1,455 @@
+/*
+ * clientMessage.cpp
+ *
+ *  Created on: Jan 16, 2019
+ *      Author: emanuele
+ */
+
+
+#include "clientMessage.h"
+
+
+namespace subpackage {
+
+	clientSubpackage::clientSubpackage() {
+	}
+
+	clientSubpackage::~clientSubpackage() {
+	}
+
+	uint32_t clientSubpackage::parse() {
+		return 0;
+	}
+
+	versionSub::~versionSub() {
+	}
+
+	configurationSub::~configurationSub() {
+	}
+
+	calibrationSub::~calibrationSub() {
+	}
+
+	robotModeSub::~robotModeSub() {
+	}
+
+	forceModeSub::~forceModeSub() {
+	}
+
+	kineInfoSub::~kineInfoSub() {
+	}
+
+	jointDataSub::~jointDataSub() {
+	}
+
+	cartesianInfoSub::~cartesianInfoSub() {
+	}
+
+	additionalInfoSub::~additionalInfoSub() {
+	}
+
+	toolDataSub::~toolDataSub() {
+	}
+
+	toolModeSub::~toolModeSub() {
+	}
+
+	toolComSub::~toolComSub() {
+	}
+
+	masterBoardSub::~masterBoardSub() {
+	}
+
+	uint32_t subpackage::clientSubpackage::getUInt32(unsigned char** dataPointer) {
+		uint32_t d;
+		memcpy(&d, *dataPointer, sizeof(uint32_t));
+		*dataPointer+=sizeof(uint32_t);
+		return big_to_native(d);
+	}
+
+	uint64_t subpackage::clientSubpackage::getUInt64(unsigned char** dataPointer) {
+		uint64_t d;
+		memcpy(&d, *dataPointer, sizeof(uint64_t));
+		*dataPointer+=sizeof(uint64_t);
+		return big_to_native(d);
+
+	}
+
+	int32_t subpackage::clientSubpackage::getInt32(unsigned char** dataPointer) {
+		int32_t d;
+		memcpy(&d, *dataPointer, sizeof(int32_t));
+		*dataPointer+=sizeof(int32_t);
+		return big_to_native(d);
+	}
+
+	double subpackage::clientSubpackage::getDouble(unsigned char** dataPointer) {
+		union{
+			uint64_t i;
+			double dd;
+		} d;
+		memcpy(&d, *dataPointer, sizeof(double));
+		d.i=big_to_native(d.i);
+		*dataPointer+=sizeof(double);
+		return d.dd;
+	}
+
+	float subpackage::clientSubpackage::getFloat(unsigned char** dataPointer) {
+		union{
+			uint32_t i;
+			float dd;
+		} d;
+		memcpy(&d, *dataPointer, sizeof(float));
+		d.i=big_to_native(d.i);
+		*dataPointer+=sizeof(float);
+		return d.dd;
+	}
+
+	char subpackage::clientSubpackage::getChar(unsigned char** dataPointer) {
+		char val=**dataPointer;
+		*dataPointer+=1;
+		return val;
+	}
+
+	unsigned char subpackage::clientSubpackage::getUChar(unsigned char** dataPointer) {
+		unsigned char val=**dataPointer;
+		*dataPointer+=1;
+		return val;
+	}
+
+	void clientSubpackage::copyString(std::string **out, unsigned char** in, char len) {
+		char* s=NULL;
+		if (*out != NULL)
+			delete(out);
+		s = (char*)malloc(len+1);
+		strncpy(s, (const char*)*in, len);
+		s[(unsigned char)len]='\0';
+		*out = new std::string(s);
+		free(s);
+		*in+=len;
+
+	}
+	//////////////////////////////////////////////////////////////////////////////
+	/////////////// Overload of Parse Method /////////////////////////////////////
+	//////////////////////////////////////////////////////////////////////////////
+	uint32_t subpackage::robotModeSub::parse() {
+		robotModeData_t *v = &dataOut->robotMode;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->timestamp = this->getUInt64(&dataIn);
+		v->isRealRobotConnected = this->getUChar(&dataIn);
+		v->isRealRobotEnabled =  this->getUChar(&dataIn);
+		v->isRobotPowerOn  = this->getUChar(&dataIn);
+		v->isEmergencyStopped = this->getUChar(&dataIn);
+		v->isProtectiveStopped = this->getUChar(&dataIn);
+		v->isProgramRunning = this->getUChar(&dataIn);
+		v->isProgramPaused = this->getUChar(&dataIn);
+		v->robotMode = this->getUChar(&dataIn);
+		v->controlMode = this->getUChar(&dataIn);
+		v->targetSpeedFraction = this->getDouble(&dataIn);
+		v->speedScaling = this->getDouble(&dataIn);
+		v->targetSpeedFractionLimit = this->getDouble(&dataIn);
+		v->internalValue = this->getUChar(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+		}
+
+	uint32_t subpackage::jointDataSub::parse() {
+		jointData_t *v = &dataOut->jointData;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		uint32_t c;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		for (c=0; c<JOINT_NUMBER; c++){
+			v->jointParam[c].actual = this->getDouble(&dataIn);
+			v->jointParam[c].target = this->getDouble(&dataIn);
+			v->jointParam[c].speedActual = this->getDouble(&dataIn);
+			v->jointParam[c].current = this->getFloat(&dataIn);
+			v->jointParam[c].voltage = this->getFloat(&dataIn);
+			v->jointParam[c].motorTemp = this->getFloat(&dataIn);
+			v->jointParam[c].nc = this->getFloat(&dataIn);
+			v->jointParam[c].mode = this->getUChar(&dataIn);
+		}
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+		}
+
+	uint32_t subpackage::cartesianInfoSub::parse() {
+		cartesianInfo_t *v = &dataOut->cartesianInfo;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		uint32_t c;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		for (c=0; c<JOINT_NUMBER; c++){
+            v->cartPosition.pos[c] = this->getDouble(&dataIn);
+		}
+		for (c=0; c<JOINT_NUMBER; c++){
+            v->cartTCPoffset.ofst[c] = this->getDouble(&dataIn);
+		}
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::masterBoardSub::parse() {
+		masterBoardData_t *v = &dataOut->masterBoard;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->digitalInputBits = this->getUInt32(&dataIn);
+		v->digitalOutputBits = this->getUInt32(&dataIn);
+		v->analogInputRange0 = this->getUChar(&dataIn);
+		v->analogInputRange1 = this->getUChar(&dataIn);
+		v->analogInput0 = this->getDouble(&dataIn);
+		v->analogInput1 = this->getDouble(&dataIn);
+		v->analogOutputDomain0 = this->getChar(&dataIn);
+		v->analogOutputDomain1 = this->getChar(&dataIn);
+		v->analogOutput0 = this->getDouble(&dataIn);
+		v->analogOutput1 = this->getDouble(&dataIn);
+		v->masterBoardTemperature = this->getFloat(&dataIn);
+		v->robotVoltage48V = this->getFloat(&dataIn);
+		v->robotCurrent = this->getFloat(&dataIn);
+		v->masterIOCurrent = this->getFloat(&dataIn);
+		v->safetyMode = this->getUChar(&dataIn);
+		v->InReducedMode = this->getUChar(&dataIn);
+		v->euromap67InterfaceInstalled = this->getChar(&dataIn);
+		#ifdef EUROMAP
+		v->euromapInputBits = this->getUInt32(&dataIn);
+		v->euromapOutputBits = this->getUInt32(&dataIn);
+		v->euromapVoltage24V = this->getFloat(&dataIn);
+		v->euromapCurrent = this->getFloat(&dataIn);
+		#endif
+		v->nc = this->getUInt32(&dataIn);
+		v->operationalModeSelectorInput = this->getUChar(&dataIn);
+		v->threePositionEnblingDeviceInput = this->getUChar(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::toolDataSub::parse() {
+		toolData_t *v = &dataOut->toolData;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->analogInputRange0 = this->getUChar(&dataIn);
+		v->analogInputRange1 = this->getUChar(&dataIn);
+		v->analogInput0 = this->getDouble(&dataIn);
+		v->analogInput1 = this->getDouble(&dataIn);
+		v->toolVoltage48V = this->getFloat(&dataIn);
+		v->toolOutputVoltage = this->getUChar(&dataIn);
+		v->toolCurrent = this->getFloat(&dataIn);
+		v->toolTemperature = this->getFloat(&dataIn);
+		v->toolMode = this->getUChar(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::kineInfoSub::parse() {
+		kinematicsInfo_t *v = &dataOut->kineInfo;
+		kineDH_t *k=&dataOut->kineInfo.kinematics;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		uint32_t c;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		for(c=0; c<JOINT_NUMBER; c++){
+			v->checksum[c]=this->getUInt32(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHtheta[c]=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHa[c]=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHd[c]=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHalpha[c]=this->getDouble(&dataIn);
+		}
+		v->calibration_status = this->getUInt32(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::configurationSub::parse() {
+		configurationData_t *v = &dataOut->configuration;
+		kineDH_t *k=&dataOut->configuration.kinematics;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		uint32_t c;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		for(c=0; c<JOINT_NUMBER; c++){
+			v->jointPositionLimits[c].min=this->getDouble(&dataIn);
+			v->jointPositionLimits[c].max=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			v->jointSpeedLimits[c].min=this->getDouble(&dataIn);
+			v->jointSpeedLimits[c].max=this->getDouble(&dataIn);
+		}
+		v->vJointDefault=this->getDouble(&dataIn);
+		v->aJointDefault=this->getDouble(&dataIn);
+		v->vToolDefault=this->getDouble(&dataIn);
+		v->aToolDefault=this->getDouble(&dataIn);
+		v->eqRadius=this->getDouble(&dataIn);
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHa[c]=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHd[c]=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHalpha[c]=this->getDouble(&dataIn);
+		}
+		for(c=0; c<JOINT_NUMBER; c++){
+			k->DHtheta[c]=this->getDouble(&dataIn);
+		}
+		v->masterboardVersion=this->getUInt32(&dataIn);
+		v->controllerBoxType=this->getInt32(&dataIn);
+		v->robotType=this->getInt32(&dataIn);
+		v->robotSubType=this->getInt32(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::forceModeSub::parse() {
+		forceModeData_t *v = &dataOut->forceMode;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		uint32_t c;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		for(c=0; c<JOINT_NUMBER; c++){
+			v->forcePosition.force[c] = this->getDouble(&dataIn);
+		}
+		v->robotDexterity = this->getDouble(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::additionalInfoSub::parse() {
+		additionalInfo_t *v = &dataOut->additionalInfo;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->feedriveButtonPressed = this->getUChar(&dataIn);
+		v->feedriveButtonEnabled = this->getUChar(&dataIn);
+		v->ioEnableFeedrive = this->getUChar(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::calibrationSub::parse() {
+		calibrationData_t *v = &dataOut->calibrationData;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		uint32_t c;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		for(c=0; c<JOINT_NUMBER; c++){
+			v->fcalData.force[c] = this->getDouble(&dataIn);
+		}
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::toolComSub::parse() {
+		toolCommunicationInfo_t *v = &dataOut->toolCom;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->communicationIsEnabled = this->getUChar(&dataIn);
+		v->baudRate = this->getInt32(&dataIn);
+		v->parity = this->getInt32(&dataIn);
+		v->stopBits = this->getInt32(&dataIn);
+		v->rxIdleChars = this->getFloat(&dataIn);
+		v->txIdleChars = this->getFloat(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::toolModeSub::parse() {
+		toolModeInfo_t *v = &dataOut->toolMode;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->outputMode = this->getChar(&dataIn);
+		v->digitalModeOut0 = this->getUChar(&dataIn);
+		v->digitalModeOut1 = this->getUChar(&dataIn);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+	uint32_t subpackage::versionSub::parse() {
+		versionMessage_t *v = &dataOut->versonMessage;
+		unsigned char* begin = dataIn;
+		uint32_t size;
+		char nameSize;
+		size = v->packageSize = this->getInt32(&dataIn);
+		v->packageType = this->getUChar(&dataIn);
+		v->timestamp = this->getUInt64(&dataIn);
+		v->source = this->getChar(&dataIn);
+		v->robotMessageType = this->getChar(&dataIn);
+
+		nameSize = v->projectNameSize = this->getChar(&dataIn);
+		this->copyString(&v->projectName, &dataIn, nameSize);
+
+		v->majorVersion = this->getUChar(&dataIn);
+		v->minorVersion = this->getUChar(&dataIn);
+		v->bugFixVersion = this->getUChar(&dataIn);
+		v->buildNumber = this->getUChar(&dataIn);
+
+		nameSize = (begin+v->packageSize-dataIn);
+		this->copyString(&v->buildDate, &dataIn, nameSize);
+
+		// check if read data is aligned with package size
+		if (dataIn-begin != size) throw dataIn-begin-size;
+		return dataIn-begin;
+	}
+
+} /* namespace clientmessage */
+
--- a/libURcom/clientMessage.h
+++ b/libURcom/clientMessage.h
@@ -0,0 +1,182 @@
+/*
+ * clientMessage.h
+ *
+ *  Created on: Jan 16, 2019
+ *      Author: emanuele
+ */
+
+#ifndef CLIENTMESSAGE_H_
+#define CLIENTMESSAGE_H_
+
+#include "packagetypes.h"
+#include <string>
+#include <arpa/inet.h>
+#include <boost/endian/conversion.hpp>
+
+namespace subpackage {
+
+	class clientSubpackage {
+		public:
+			clientSubpackage();
+			virtual ~clientSubpackage();
+
+			unsigned char* dataIn;
+			clientData_t* dataOut;
+
+			virtual uint32_t parse();
+
+			void copyString(std::string** out, unsigned char** in, char len);
+			uint32_t getUInt32(unsigned char** dataPointer);
+			uint64_t getUInt64(unsigned char** dataPointer);
+			int32_t	 getInt32(unsigned char** dataPointer);
+			double	 getDouble(unsigned char** dataPointer);
+			float    getFloat(unsigned char** dataPointer);
+			char	 getChar(unsigned char** dataPointer);
+			unsigned char getUChar(unsigned char** dataPointer);
+	};
+
+	class robotModeSub: public clientSubpackage  {
+		public:
+			robotModeSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~robotModeSub();
+
+			uint32_t parse();
+		};
+
+	class jointDataSub: public clientSubpackage  {
+		public:
+			jointDataSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~jointDataSub();
+
+			uint32_t parse();
+		};
+
+	class cartesianInfoSub: public clientSubpackage  {
+		public:
+			cartesianInfoSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~cartesianInfoSub();
+
+			uint32_t parse();
+	};
+
+	class masterBoardSub: public clientSubpackage  {
+		public:
+			masterBoardSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~masterBoardSub();
+
+			uint32_t parse();
+	};
+
+	class toolDataSub: public clientSubpackage  {
+		public:
+			toolDataSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~toolDataSub();
+
+			uint32_t parse();
+	};
+
+	class kineInfoSub: public clientSubpackage  {
+		public:
+			kineInfoSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~kineInfoSub();
+
+			uint32_t parse();
+	};
+
+	class configurationSub: public clientSubpackage {
+		public:
+			configurationSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~configurationSub();
+
+			uint32_t parse();
+	};
+
+	class forceModeSub: public clientSubpackage  {
+		public:
+			forceModeSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~forceModeSub();
+
+			uint32_t parse();
+	};
+
+	class additionalInfoSub: public clientSubpackage  {
+		public:
+			additionalInfoSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~additionalInfoSub();
+
+			uint32_t parse();
+	};
+
+	class calibrationSub: public clientSubpackage  {
+		public:
+			calibrationSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~calibrationSub();
+
+			uint32_t parse();
+	};
+
+	class toolComSub: public clientSubpackage  {
+		public:
+			toolComSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~toolComSub();
+
+			uint32_t parse();
+	};
+
+	class toolModeSub: public clientSubpackage  {
+		public:
+			toolModeSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout;
+			};
+			virtual ~toolModeSub();
+
+			uint32_t parse();
+	};
+
+	class versionSub: public clientSubpackage {
+		public:
+			versionSub(unsigned char* din, clientData_t* dout) {
+				dataIn=din;
+				dataOut=dout; };
+			virtual ~versionSub();
+
+			uint32_t parse();
+	};
+
+} /* namespace clientmessage */
+
+#endif /* CLIENTMESSAGE_H_ */
--- a/libURcom/packagetypes.h
+++ b/libURcom/packagetypes.h
@@ -0,0 +1,349 @@
+/*
+ * packagetypes.h
+ *
+ *  Created on: Jan 14, 2019
+ *      Author: emanuele
+ *
+ *  Strutture e definizioni dei pacchetti utilizzati dal protocollo Client Interface di UR
+ *
+ */
+#include <cstdint>
+#include <ctime>
+#include <boost/endian/buffers.hpp>
+using namespace boost::endian;
+
+#define HEADERSIZE 4
+
+//#define EUROMAP
+#define JOINT_NUMBER		6
+
+// PACKET TYPES
+#define VERSION_MESSAGE		20
+#define ROBOT_MESSAGE		16
+// SUBPACKET TYPES
+#define ROBOT_MODE_DATA 	0
+#define JOINT_DATA 			1
+#define TOOL_DATA 			2
+#define MASTER_BOARD_DATA 	3
+#define CARTESIAN_INFO 		4
+#define KINEMATICS_INFO 	5
+#define CONFIGURATION_DATA 	6
+#define FORCE_MODE_DATA 	7
+#define ADDITIONAL_INFO 	8
+#define CALIBRATION_DATA	9
+#define SAFETY_DATA			10
+#define TOOL_COM_INFO 		11
+#define TOOL_MODE_INFO 		12
+
+enum {
+	ROBOT_MODE_NO_CONTROLLER = -1,
+	ROBOT_MODE_DISCONNECTED,
+	ROBOT_MODE_CONFIRM_SAFETY,
+	ROBOT_MODE_BOOTING,
+	ROBOT_MODE_POWER_OFF,
+	ROBOT_MODE_POWER_ON,
+	ROBOT_MODE_IDLE,
+	ROBOT_MODE_BACKDRIVE,
+	ROBOT_MODE_RUNNING,
+	ROBOT_MODE_UPDATING_FIRMWARE,
+} robotModes;
+
+enum {
+	CONTROL_MODE_POSITION = 0,
+	CONTROL_MODE_TEACH,
+	CONTROL_MODE_FORCE,
+	CONTROL_MODE_TORQUE,
+} controlModes;
+
+enum {
+    JOINT_MODE_RESET =235,
+    JOINT_MODE_SHUTTING_DOWN,
+    JOINT_PART_D_CALIBRATION_MODE,
+    JOINT_MODE_BACKDRIVE,
+    JOINT_MODE_POWER_OFF,
+    JOINT_MODE_READY_FOR_POWER_OFF,
+    JOINT_MODE_NOT_RESPONDING,
+    JOINT_MODE_MOTOR_INITIALISATION,
+    JOINT_MODE_BOOTING,
+    JOINT_PART_D_CALIBRATION_ERROR_MODE,
+    JOINT_MODE_BOOTLOADER,
+    JOINT_CALIBRATION_MODE,
+    JOINT_MODE_VIOLATION,
+    JOINT_MODE_FAULT,
+    JOINT_MODE_RUNNING,
+    JOINT_MODE_IDLE
+} jointModes;
+
+enum {
+    SAFETY_MODE_NORMAL = 1,
+    SAFETY_MODE_REDUCED,
+    SAFETY_MODE_PROTECTIVE_STOP,
+    SAFETY_MODE_RECOVERY,
+    SAFETY_MODE_SAFEGUARD_STOP,
+    SAFETY_MODE_SYSTEM_EMERGENCY_STOP,
+    SAFETY_MODE_ROBOT_EMERGENCY_STOP,
+    SAFETY_MODE_VIOLATION,
+    SAFETY_MODE_FAULT,
+    SAFETY_MODE_VALIDATE_JOINT_ID,
+    SAFETY_MODE_UNDEFINED_SAFETY_MODE
+} safetyModes;
+
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	uint64_t timestamp;
+	char source;
+	char robotMessageType;
+	char projectNameSize;
+	std::string *projectName=NULL;
+	unsigned char majorVersion;
+	unsigned char minorVersion;
+	unsigned char bugFixVersion;
+	unsigned char buildNumber;
+	std::string *buildDate=NULL;
+} versionMessage_t;
+
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	int64_t timestamp;
+	unsigned char isRealRobotConnected;
+	unsigned char isRealRobotEnabled;
+	unsigned char isRobotPowerOn;
+	unsigned char isEmergencyStopped;
+	unsigned char isProtectiveStopped;
+	unsigned char isProgramRunning;
+	unsigned char isProgramPaused;
+	unsigned char robotMode;
+	unsigned char controlMode;
+	double targetSpeedFraction;
+	double speedScaling;
+	double targetSpeedFractionLimit;
+	unsigned char internalValue;
+} robotModeData_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	struct {
+		double actual;
+		double target;
+		double speedActual;
+		float current;
+		float voltage;
+		float motorTemp;
+		float nc;
+		unsigned char mode;
+	} jointParam [JOINT_NUMBER];
+} jointData_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	union {
+		struct{
+			double X;
+			double Y;
+			double Z;
+			double RX;
+			double RY;
+			double RZ;
+		};
+		double pos [6];
+	} cartPosition;
+	union {
+		struct{
+			double ofstX;
+			double ofstY;
+			double ofstZ;
+			double ofstRX;
+			double ofstRY;
+			double ofstRZ;
+		};
+		double ofst [6];
+	} cartTCPoffset;
+} cartesianInfo_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	uint32_t digitalInputBits;
+	uint32_t digitalOutputBits;
+	unsigned char analogInputRange0;
+	unsigned char analogInputRange1;
+	double analogInput0;
+	double analogInput1;
+	char analogOutputDomain0;
+	char analogOutputDomain1;
+	double analogOutput0;
+	double analogOutput1;
+	float masterBoardTemperature;
+	float robotVoltage48V;
+	float robotCurrent;
+	float masterIOCurrent;
+	unsigned char safetyMode;
+	unsigned char InReducedMode;
+	char euromap67InterfaceInstalled;
+	#ifdef EUROMAP
+	  uint32_t euromapInputBits;
+	  uint32_t euromapOutputBits;
+	  float euromapVoltage24V;
+	  float euromapCurrent;
+	#endif
+	uint32_t nc;
+	unsigned char operationalModeSelectorInput;
+	unsigned char threePositionEnblingDeviceInput;
+} masterBoardData_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	unsigned char analogInputRange0;
+	unsigned char analogInputRange1;
+	double analogInput0;
+	double analogInput1;
+	float toolVoltage48V;
+	unsigned char toolOutputVoltage;
+	float toolCurrent;
+	float toolTemperature;
+	unsigned char toolMode;
+} toolData_t;
+
+
+typedef struct {
+	double DHa [JOINT_NUMBER];
+	double DHd [JOINT_NUMBER];
+	double DHalpha [JOINT_NUMBER];
+	double DHtheta [JOINT_NUMBER];
+} kineDH_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	uint32_t checksum [JOINT_NUMBER];
+	kineDH_t kinematics;
+	unsigned char calibration_status;
+} kinematicsInfo_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	struct {
+		double min;
+		double max;
+	} jointPositionLimits [JOINT_NUMBER];
+	struct {
+		double min;
+		double max;
+	} jointSpeedLimits [JOINT_NUMBER];
+	double vJointDefault;
+	double aJointDefault;
+	double vToolDefault;
+	double aToolDefault;
+	double eqRadius;
+	kineDH_t kinematics;
+	int32_t masterboardVersion;
+	int32_t controllerBoxType;
+	int32_t robotType;
+	int32_t robotSubType;
+} configurationData_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	union {
+		struct{
+			double FX;
+			double FY;
+			double FZ;
+			double FRX;
+			double FRY;
+			double FRZ;
+		};
+		double force[6];
+	} forcePosition;
+	double robotDexterity;
+} forceModeData_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	unsigned char feedriveButtonPressed;
+	unsigned char feedriveButtonEnabled;
+	unsigned char ioEnableFeedrive;
+} additionalInfo_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	union {
+		struct{
+			double FX;
+			double FY;
+			double FZ;
+			double FRX;
+			double FRY;
+			double FRZ;
+		};
+		double force[6];
+	} fcalData;
+} calibrationData_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	unsigned char communicationIsEnabled;
+	int32_t baudRate;
+	int32_t parity;
+	int32_t stopBits;
+	float rxIdleChars;
+	float txIdleChars;
+} toolCommunicationInfo_t;
+
+typedef struct {
+	int32_t packageSize;
+	unsigned char packageType;
+	unsigned char outputMode;
+	unsigned char digitalModeOut0;
+	unsigned char digitalModeOut1;
+} toolModeInfo_t;
+
+typedef struct {
+	versionMessage_t 	versonMessage;
+	configurationData_t configuration;
+	calibrationData_t 	calibrationData;
+	robotModeData_t		robotMode;
+	forceModeData_t 	forceMode;
+	kinematicsInfo_t	kineInfo;
+	jointData_t 		jointData;
+	cartesianInfo_t 	cartesianInfo;
+	additionalInfo_t 	additionalInfo;
+	toolData_t 			toolData;
+	toolModeInfo_t 		toolMode;
+	toolCommunicationInfo_t toolCom;
+	masterBoardData_t 	masterBoard;
+	time_t 				lastUpdate;
+} clientData_t;
+
+
+typedef struct {
+	union{
+		struct {
+			double X,Y,Z,RX,RY,RZ;
+		};
+		double p[JOINT_NUMBER];
+	};
+} pose_t;
+
+typedef struct {
+	union{
+		struct {
+			double J1,J2,J3,J4,J5,J6;
+		};
+		double p[JOINT_NUMBER];
+	};
+} jointPose_t;
+
+