Obbart/RoboGlue_QT
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Commit Iniziale

Browse Source
This commit is contained in:
Emanuele
2019-10-21 11:37:31 +02:00
commit 44ac6e8802
56 changed files with 32022 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

258
libURcom/URCLinterface.cpp Normal file
View File

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