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); }