Commit Iniziale
This commit is contained in:
455
libURcom/clientMessage.cpp
Normal file
455
libURcom/clientMessage.cpp
Normal file
@@ -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 */
|
||||
|
||||
Reference in New Issue
Block a user