259 lines
9.2 KiB
C++
259 lines
9.2 KiB
C++
/*
|
|
* 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")
|
|
}
|
|
}
|
|
|