void ControlQueue::rollBack(double time) {
rollbackMode = false;
int rollBackCount = (int) (time / getCycleTime());
int stretchFactor = ceil((double) rollBackCount / (double) rollBackQueue.size());
stretchFactor = max((double) stretchFactor, 1.0);
vec lastCommand(getMovementDegreesOfFreedom());
if(rollBackQueue.size())
lastCommand = rollBackQueue.front();
int newRollBackCount = ceil((double) rollBackCount / (double) stretchFactor);
// fill command queue with last commands (backwards)
for(int i = 0; i < newRollBackCount && rollBackQueue.size(); ++i) {
vec nextCommand = rollBackQueue.front();
// interpolate to stretch the trajectory in case there are not enough measured packets (happens in usage with simulator)
vec diffUnit = (nextCommand - lastCommand) / (double) stretchFactor;
for(int j = 0; j < stretchFactor; ++j) {
move(lastCommand + j * diffUnit);
}
lastCommand = nextCommand;
rollBackQueue.pop_front();
}
// wait until everything has been executed
synchronizeToQueue(1);
rollBackQueue.clear();
}
void ControlQueue::startRollBackMode(double possibleTime) {
rollBackQueue.clear();
rollbackMode = true;
rollBackTime = possibleTime;
// buffer of 1.0 more second
rollBackQueueSize = (int) ((possibleTime + 1.0) / getCycleTime());
}
void ControlQueue::jointsCollector() {
double lastTime = DBL_MIN;
collectedJoints.clear();
ros::Rate r((int) (1.0 / getCycleTime() + 10.0));
while(continueCollecting) {
mes_result lastResult = getCurrentJoints();
if(lastResult.time != lastTime)
collectedJoints.push_back(lastResult);
r.sleep();
}
}
void ControlQueue::run() {
setInitValuesInternal();
ros::Rate sleepRate(1.0 / sleepTime);
arma::vec movement = arma::vec(1);
geometry_msgs::Pose movementPose;
if(getStartingJoints().n_elem > 1) {
if(!isShutUp())
ROS_INFO("(ControlQueue) start moving to start position");
jointPtp(getStartingJoints());
if(!isShutUp())
ROS_INFO("(ControlQueue) finished moving to start position");
}
isInit = true;
lastDuration = 0.0;
double toleratedMaxDuration = 1.1 * getCycleTime();
double toleratedMinDuration = 0.9 * getCycleTime();
movement = getCurrentJoints().joints;
t.tic("r");
while(!finish && ros::ok) {
t.tic("d");
/*
// switched off adaptive cycle time behaviour for now - probably requires a PID controller for that
if(toleratedMinDuration < lastDuration || lastDuration > toleratedMaxDuration) {
sleepTime = max(0.000000001, sleepTime + 0.3 * (desiredCycleTime - lastDuration));
sleepRate = ros::Rate(1.0 / sleepTime);
}
*/
currentTime = t.toc("r");
currentJoints = getCurrentJoints().joints;
currentCartPose = getCurrentCartesianPose();
if(rollbackMode) {
rollBackQueue.push_front(currentJoints);
// if queue is full --> go back
while(rollBackQueue.size() > rollBackQueueSize)
rollBackQueue.pop_back();
}
if(!stopQueueWhilePtp() || !jointPtpRunning && !cartesianPtpRunning) {
if(currentControlType == CONTROLQUEUE_JNT_IMP_MODE || currentControlType == CONTROLQUEUE_JNT_POS_MODE) {
if(movementQueue.size() > 0) {
// move to position in queue
movement = movementQueue.front();
movementQueue.pop();
} else {
if(stopQueueWhilePtp())
movement = getCurrentJoints().joints;
}
submitNextJointMove(movement);
} else if(currentControlType == CONTROLQUEUE_CART_IMP_MODE) {
if(cartesianMovementQueue.size() > 0) {
// move to position in queue
movementPose = cartesianMovementQueue.front();
cartesianMovementQueue.pop();
} else {
movementPose = getCurrentCartesianPose();
}
submitNextCartMove(movementPose);
}
}
sleepRate.sleep();
ros::spinOnce();
lastDuration = t.toc("d");
}
if(!isShutUp())
cout << "thread finished" << endl;
}
int procGetUnit(unitPtr uPtr,char *recvBuf,int recvLen,char *result,char bitpos,char *pRecvPtr) {
char string[256];
char error[1000];
char buffer[MAXBUF];
char *errPtr=error;
/* short t; */
float erg;
int ergI;
char formatI[20];
float floatV=0;
char *inPtr;
char *tPtr;
/* hier die Typen fuer die Umrechnung in <type> Tag */
int8_t charV;
uint8_t ucharV;
int16_t shortV;
int16_t tmpS;
uint16_t ushortV;
uint16_t tmpUS;
int32_t intV;
int32_t tmpI;
uint32_t tmpUI;
uint32_t uintV;
bzero(errPtr,sizeof(error));
/* wir behandeln die verschiedenen <type> Eintraege */
if (strstr(uPtr->type,"cycletime")==uPtr->type) { /* Schaltzeit */
if (getCycleTime(recvBuf,recvLen,result))
return(1);
else
return(-1);
}
else if (strstr(uPtr->type,"systime")==uPtr->type) { /* Systemzeit */
if (getSysTime(recvBuf,recvLen,result))
return(1);
else
return(-1);
}
else if (strstr(uPtr->type,"errstate")==uPtr->type) { /* Errrocode + Systemzeit */
if (getErrState(uPtr->ePtr,recvBuf,recvLen,result))
return(1);
else
return(-1);
}
else if (strstr(uPtr->type,"enum")==uPtr->type) { /*enum*/
if(bytes2Enum(uPtr->ePtr,recvBuf,&tPtr,recvLen)) {
strcpy(result,tPtr);
return(1);
}
else {
sprintf(result,"Kein passendes Enum gefunden");
return(-1);
}
}
/* hier kommen die ganzen numerischen Typen */
if (strstr(uPtr->type,"char")==uPtr->type) { /* Umrechnung in Char 1Byte */
memcpy(&charV,recvBuf,1);
floatV=charV; /* impliziete Typumnwandlung nach float fuer unsere Arithmetic */
sprintf(formatI,"%%02X %%s");
}
else if (strstr(uPtr->type,"uchar")==uPtr->type) { /* Umrechnung in Unsigned Char 1Byte */
memcpy(&ucharV,recvBuf,1);
floatV=ucharV; /* impliziete Typumnwandlung nach float fuer unsere Arithmetic */
sprintf(formatI,"%%02X %%s");
}
else if (strstr(uPtr->type,"short")==uPtr->type) { /* Umrechnung in Short 2Byte */
memcpy(&tmpS,recvBuf,2);
/* je nach CPU Typ wird hier die Wandlung vorgenommen */
shortV=__le16_to_cpu(tmpS);
floatV=shortV; /* impliziete Typumnwandlung nach float fuer unsere Arithmetic */
sprintf(formatI,"%%04X %%s");
}
else if (strstr(uPtr->type,"ushort")==uPtr->type) { /* Umrechnung in Short 2Byte */
memcpy(&tmpUS,recvBuf,2);
/* je nach CPU Typ wird hier die Wandlung vorgenommen */
ushortV=__le16_to_cpu(tmpUS);
floatV=ushortV; /* impliziete Typumnwandlung nach float fuer unsere Arithmetic */
sprintf(formatI,"%%04X %%s");
}
else if (strstr(uPtr->type,"int")==uPtr->type) { /* Umrechnung in Int 4Byte */
memcpy(&tmpI,recvBuf,4);
/* je nach CPU Typ wird hier die Wandlung vorgenommen */
intV=__le32_to_cpu(tmpI);
floatV=intV; /* impliziete Typumnwandlung nach float fuer unsere Arithmetic */
sprintf(formatI,"%%08X %%s");
}
else if (strstr(uPtr->type,"uint")==uPtr->type) { /* Umrechnung in Unsigned Int 4Byte */
memcpy(&tmpUI,recvBuf,4);
/* je nach CPU Typ wird hier die Wandlung vorgenommen */
uintV=__le32_to_cpu(tmpUI);
floatV=uintV; /* impliziete Typumnwandlung nach float fuer unsere Arithmetic */
sprintf(formatI,"%%08X %%s");
}
else if (uPtr->type) {
bzero(string,sizeof(string));
snprintf(string, sizeof(string),"Unbekannter Typ %s in Unit %s",uPtr->type,uPtr->name);
logIT(LOG_ERR,string);
return(-1);
}
/* etwas logging */
int n;
char *ptr;
char res;
ptr=recvBuf;
bzero(buffer,sizeof(buffer));
for(n=0;n<=9;n++) {/* Bytes 0..9 sind von Interesse */
bzero(string,sizeof(string));
unsigned char byte=*ptr++ & 255;
snprintf(string, sizeof(string),"B%d:%02X ",n,byte);
strcat(buffer,string);
if (n >= MAXBUF-3)
break;
}
if (uPtr->gCalc && *uPtr->gCalc) { /* <calc im XML und get darin definiert */
snprintf(string, sizeof(string),"Typ: %s (in float: %f)",uPtr->type,floatV);
logIT(LOG_INFO,string);
inPtr=uPtr->gCalc;
snprintf(string, sizeof(string),"(FLOAT) Exp:%s [%s]",inPtr,buffer);
logIT(LOG_INFO,string);
erg=execExpression(&inPtr,recvBuf,floatV,errPtr);
if (*errPtr) {
snprintf(string, sizeof(string),"Exec %s: %s",uPtr->gCalc,error);
logIT(LOG_ERR,string);
strcpy(result,string);
return(-1);
}
sprintf(result,"%f %s",erg,uPtr->entity);
}
else if (uPtr->gICalc && *uPtr->gICalc) { /* <icalc im XML und get darin definiert */
inPtr=uPtr->gICalc;
snprintf(string, sizeof(string),"(INT) Exp:%s [BP:%d] [%s]",inPtr,bitpos,buffer);
logIT(LOG_INFO,string);
ergI=execIExpression(&inPtr,recvBuf,bitpos,pRecvPtr,errPtr);
if (*errPtr) {
snprintf(string, sizeof(string),"Exec %s: %s",uPtr->gCalc,error);
logIT(LOG_ERR,string);
strcpy(result,string);
return(-1);
}
snprintf(string, sizeof(string),"Erg: (Hex max. 4Byte) %08x",ergI);
logIT(LOG_INFO,string);
res=ergI;
if( uPtr->ePtr && bytes2Enum(uPtr->ePtr,&res,&tPtr,recvLen)) {
strcpy(result,tPtr);
return(1);
}
else {
sprintf(result,formatI,ergI,uPtr->entity);
return(1);
}
/* hier noch das durchsuchen der enums ggf durchfuehren */
}
return(1);
}
