FRIComm.cc
Go to the documentation of this file.
00001 
00002 #include <unistd.h>
00003 #include <stdio.h>
00004 #include <math.h>
00005 
00006 #include <string.h>
00007 
00008 #include <pthread.h>
00009 
00010 // network
00011 #include <sys/socket.h>
00012 #include <netinet/in.h>
00013 #include <arpa/inet.h>
00014 #include <errno.h>
00015 
00016 #include "FRIComm.hh"
00017 #include "FRICheck.hh"
00018 #include "CartesianImpedance.hh"
00019 
00020 // DEBUGGING
00021 FILE *LOG=0;
00022 
00023 
00024 #define DEG *M_PI/180.0
00025 #define RAD /M_PI*180.0
00026 
00027 // threading helper class
00028 class pthread_scoped_lock
00029 {
00030 public:
00031   pthread_scoped_lock(pthread_mutex_t *mutex) : mutex_(mutex) { pthread_mutex_lock(mutex_); }
00032   ~pthread_scoped_lock() { unlock(); }
00033   void unlock() { pthread_mutex_unlock(mutex_); }
00034 private:
00035   pthread_mutex_t *mutex_;
00036 };
00037 
00038 
00039 RobotData::RobotData() :
00040   seq(0)
00041 {
00042   for(int i=0; i < 7; i++) {
00043     commanded[i] = 0.0;
00044     position[i] = 0.0;
00045     torque[i] = 0.0;
00046     torque_raw[i] = 0.0;
00047   }
00048   for(int i=0; i < 6; i++) {
00049     torqueTCP[i] = 0.0;
00050   }
00051 }
00052 
00053 
00054 RobotCommand::RobotCommand() :
00055   useCartesianImpedance(false)
00056 {
00057   for(int i=0; i < 7; i++) {
00058     command[i] = 0.0;
00059     stiffness[i] = DEFAULT_STIFFNESS;
00060     damping[i] = DEFAULT_DAMPING;
00061     addTorque[i] = 0.0;
00062   }
00063 }
00064 
00065 
00066 void RobotCommand::incrementPosition(float* newPos, float* currentPos, float rate)
00067 {
00068   for(int i=0; i < 7; i++)
00069     newPos[i] = currentPos[i] + command[i]*rate;
00070 }
00071 
00072 
00073 RobotStatus::RobotStatus()
00074 {
00075   memset(this, 0, sizeof(*this));
00076 }
00077 
00078 
00079 CartesianImpedance::CartesianImpedance()
00080 {
00081   // set everything to zero
00082   memset(this, 0, sizeof(*this));
00083 
00084   // well, almost...
00085   for(int i=0; i < 6; i++)
00086   {
00087     K[i*6 + i] = (i < 3) ? DEFAULT_TRANS_STIFFNESS : DEFAULT_ROT_STIFFNESS;
00088     D[i*6 + i] = DEFAULT_DAMPING;
00089   }
00090 }
00091 
00092 
00093 FRIComm::FRIComm() :
00094   krl_time_(-1), seq_(0), closing_(false), runstop_(false), socket_(0),
00095   duration_sum(0), duration_num(0), duration_max(0)
00096 {
00097   local_port_ = 0;
00098   remote_address_ = "";
00099   remote_port_ = 0;
00100 
00101   safety_  = new FRICheck();
00102   safety2_ = new FRICheck();
00103   cartesianImpedance_ = new CartesianImpedanceControl();
00104 
00105 
00106   // clear data buffers
00107   memset(&fri_msr_, 0, sizeof(fri_msr_));
00108   memset(&fri_cmd_, 0, sizeof(fri_cmd_));
00109 
00110   fri_cmd_.head.datagramId = FRI_DATAGRAM_ID_CMD;
00111   fri_cmd_.head.packetSize = sizeof(tFriCmdData);
00112 
00113   // full control for joint impedance controller
00114   fri_cmd_.cmd.cmdFlags = FRI_CMD_JNTPOS | FRI_CMD_JNTSTIFF | FRI_CMD_JNTDAMP | FRI_CMD_JNTTRQ;
00115 
00116   last_tick.tv_sec = 0;
00117   last_tick.tv_usec = 0;
00118 }
00119 
00120 void FRIComm::configureNetwork(int local_port, std::string remote_address, int remote_port)
00121 {
00122   local_port_ = local_port;
00123   remote_address_ = remote_address;
00124   remote_port_ = remote_port;
00125 }
00126 
00127 FRIComm::~FRIComm()
00128 {
00129   delete safety_;
00130   delete safety2_;
00131   delete cartesianImpedance_;
00132 }
00133 
00134 
00135 bool FRIComm::open()
00136 {
00137   socket_ = socket(AF_INET, SOCK_DGRAM, 0);
00138   setsockopt(socket_, SOL_SOCKET, SO_REUSEADDR, 0, 0);
00139 
00140   // time out after there was no data for 0.1 seconds.
00141   struct timeval tv;
00142   tv.tv_sec = 0;
00143   tv.tv_usec = 100000;
00144   setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
00145 
00146   bzero((char *) &remote_addr_, sizeof(remote_addr_));
00147   remote_addr_.sin_family = AF_INET;
00148   remote_addr_.sin_addr.s_addr = inet_addr(remote_address_.c_str());
00149   remote_addr_.sin_port = htons(remote_port_);
00150 
00151   struct sockaddr_in local_addr;
00152   bzero((char *) &local_addr, sizeof(local_addr));
00153   local_addr.sin_family = AF_INET;
00154   local_addr.sin_addr.s_addr = INADDR_ANY;
00155   local_addr.sin_port = htons(local_port_);
00156 
00157   int res = bind(socket_, (sockaddr*) &local_addr, sizeof(sockaddr_in));
00158 
00159   if(res == -1) {
00160     if(errno == EADDRINUSE) {
00161       printf("# ERROR: Port %d is already taken. Is there another client running?\n", local_port_);
00162     } else {
00163       printf("# ERROR: failed to bind the UDP socket to 0.0.0.0:%d\n", local_port_);
00164     }
00165     return false;
00166   }
00167   return true; 
00168 }
00169 
00170 
00177 void FRIComm::close()
00178 {
00179   int   intData[16]  = {2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00180   float realData[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00181 
00182 
00183   if(fri_msr_.intf.state == FRI_STATE_CMD) {
00184 
00185     float last_pos[7] = {9,9,9,9,9,9,9};
00186     bool stopped=false;
00187     while(!stopped) {
00188       if(!receive()) // read may time out ...
00189         break;       // ... command mode was definitely left.
00190 
00191       // command zero velocity
00192       respond();
00193 
00194       // check if commanded position has changed due to safety (vel, acc)
00195       stopped=true;
00196       for(int i=0; i < 7; i++)
00197         stopped &= (fri_cmd_.cmd.jntPos[i] == last_pos[i]);
00198 
00199       // remember last position
00200       for(int i=0; i < 7; i++)
00201         last_pos[i] = fri_cmd_.cmd.jntPos[i];
00202     }
00203 
00204     closing_=true;
00205 
00206     postCommand(intData, realData);
00207     while(fri_msr_.intf.state == FRI_STATE_CMD 
00208       || (fri_cmd_.krl.boolData & 1) ) {
00209 
00210       if(!receive()) // read may time out ...
00211         break;       // ... command mode was definitely left.
00212 
00213       // command has timed out without effect
00214       if(krl_time_ == -1)
00215         postCommand(intData, realData);
00216 
00217       respond(); // will mirror current pos since closing_ is set
00218     }
00219 
00220     // keep up communication for a while...
00221     for(int i=0; i < 100; i++) {
00222       if(!receive()) // read may time out ...
00223         break;       // ... command mode was definitely left.
00224       respond();
00225     }
00226   }
00227   // bye, bye!
00228   ::close(socket_);
00229 
00230   closing_=false;
00231 }
00232 
00233 
00237 bool FRIComm::receive()
00238 {
00239   struct sockaddr addr;
00240   socklen_t addr_len = sizeof(addr);
00241 
00242   int n = recvfrom(socket_, (void*) &fri_msr_, sizeof(fri_msr_), 0, &addr, &addr_len);
00243 
00244   tick();
00245 
00246   if(n != sizeof(tFriMsrData)) {
00247     if(errno == EAGAIN)
00248       return false; // no packet received. return silently
00249 
00250     printf("# (%d) ERROR: %s\n", errno, strerror(errno));
00251     printf("# ERROR: bad packet length %d (expected: %d)\n", n, (int) sizeof(tFriMsrData));
00252     return false;
00253   }
00254 
00255   if(seq_ == 0) {
00256     safety_->setPos(fri_msr_.data.cmdJntPos);
00257     safety2_->setPos(fri_msr_.data.cmdJntPos);
00258   }
00259 
00260   // prepare response packet
00261   fri_cmd_.head.sendSeqCount = ++seq_;
00262   fri_cmd_.head.reflSeqCount = fri_msr_.head.sendSeqCount;
00263 
00264   return true;
00265 }
00266 
00267 
00271 void FRIComm::respond()
00272 {
00273   if(runstop_) {
00274     float* pos = safety_->pos();
00275     for(int i=0; i < 7; i++) {
00276       fri_cmd_.cmd.jntPos[i] = pos[i];
00277     }
00278   }
00279 
00280   // monitor mode settings
00281   if(fri_msr_.intf.state != FRI_STATE_CMD || fri_msr_.robot.power == 0) {
00282     for(int i=0; i < 7; i++) {
00283       fri_cmd_.cmd.jntPos[i] = fri_msr_.data.cmdJntPos[i]+fri_msr_.data.cmdJntPosFriOffset[i];
00284       fri_cmd_.cmd.addJntTrq[i] = 0.0;
00285     }
00286   }
00287 
00288   // TODO: filter stiffness, damping and torque
00289   safety_->adjust(fri_cmd_.cmd.jntPos, fri_msr_.intf.desiredCmdSampleTime, fri_msr_.intf.safetyLimits);
00290 
00291 
00292   tFriCmdData* command=0;
00293   tFriCmdData commands;
00294 
00295   // commanded position limiting to keep the KUKA controllers happy
00296   if(useCartesianImpedance_)
00297   {
00298     float cmdpos[7];
00299     for(int i=0; i < 7; i++)
00300       cmdpos[i] = fri_msr_.data.msrJntPos[i];
00301 
00302     safety2_->adjust(cmdpos, fri_msr_.intf.desiredCmdSampleTime, fri_msr_.intf.safetyLimits);
00303 
00304     commands = cartesianImpedance_->computeRobotCommand(fri_msr_, fri_cmd_,
00305                                                         cartesianImpedanceCmd_, cmdpos);
00306     command = &commands;
00307   }
00308   else
00309   {
00310     command = &fri_cmd_;
00311     safety2_->adjust(fri_cmd_.cmd.jntPos, fri_msr_.intf.desiredCmdSampleTime, fri_msr_.intf.safetyLimits);
00312   }
00313 
00314   stateHandler();
00315   processCommand();
00316 
00317   // DEBUGGING
00318   if(LOG) {
00319     fprintf(LOG, "%f   ", fri_msr_.intf.timestamp);
00320 
00321     for(int i=0; i < 7; i++)
00322       fprintf(LOG,"%5.5f ", command->cmd.addJntTrq[i]);
00323 
00324     fprintf(LOG, "        ");
00325 
00326     for(int i=0; i < 7; i++)
00327       fprintf(LOG,"%5.5f ", command->cmd.jntStiffness[i]);
00328 
00329     fprintf(LOG, "        ");
00330 
00331 //    for(int i=0; i < 7; i++)
00332 //      fprintf(LOG,"%5.5f ", command->cmd.jntPos[i]);
00333     for(int i=0; i < 7*7; i++)
00334       fprintf(LOG,"%5.5f ", fri_msr_.data.massMatrix[i]);
00335 
00336     fprintf(LOG, "        ");
00337 
00338     for(int i=0; i < 7*6; i++)
00339       fprintf(LOG,"%5.5f ", fri_msr_.data.jacobian[i]);
00340 
00341 
00342     fprintf(LOG, "\n");
00343   }
00344   // DEBUGGING END
00345 
00346 
00347   // monitor mode settings
00348   if(fri_msr_.intf.state != FRI_STATE_CMD || closing_ || fri_msr_.robot.power != 0x7f) {
00349     for(int i=0; i < 7; i++) {
00350       command->cmd.jntPos[i] = fri_msr_.data.cmdJntPos[i]+fri_msr_.data.cmdJntPosFriOffset[i];
00351       command->cmd.addJntTrq[i] = 0.0;
00352     }
00353   }
00354 
00355 
00356   // send packet
00357   sendto(socket_, (void*) command, sizeof(fri_cmd_), 0, (sockaddr*) &remote_addr_, sizeof(remote_addr_));
00358 
00359   tock();
00360 }
00361 
00362 
00363 bool FRIComm::postCommand(int iData[16], float rData[16])
00364 {
00365   if(krl_time_ > fri_msr_.intf.timestamp)
00366     return false;  // command is being processed
00367 
00368   for(int i=0; i < 16; i++) {
00369     fri_cmd_.krl.intData[i] = iData[i];
00370     fri_cmd_.krl.realData[i] = rData[i];
00371   }
00372   fri_cmd_.krl.boolData = 1;  // raise flag 0
00373   krl_time_ = fri_msr_.intf.timestamp + KRL_TIMEOUT;
00374 
00375   return true;
00376 }
00377 
00378 
00379 void FRIComm::processCommand()
00380 {
00381   if(fri_msr_.krl.boolData & 1) {
00382     // processing command, push end time
00383     fri_cmd_.krl.boolData &= 0xfffe;
00384     krl_time_ = fri_msr_.intf.timestamp + KRL_GUARD_TIME;
00385   }
00386 
00387   if(krl_time_ != -1 && krl_time_ < fri_msr_.intf.timestamp) {
00388     printf("**krl_time: %f < %f\n", krl_time_, fri_msr_.intf.timestamp);
00389     if((fri_cmd_.krl.boolData & 1)) {
00390       // timeout, command flag still set
00391       fri_cmd_.krl.boolData &= 0xfffe;
00392       krl_time_ = fri_msr_.intf.timestamp + KRL_GUARD_TIME;
00393     } else {
00394       printf("command timeout\n");
00395       // timeout, clear command
00396       for(int i=0; i < 16; i++) {
00397         fri_cmd_.krl.intData[i] = 0;
00398         fri_cmd_.krl.realData[i] = 0.0;
00399       }
00400       krl_time_ = -1;
00401     }    
00402   }
00403 }
00404 
00407 void FRIComm::stateHandler()
00408 {
00409   int   intData[16]  = {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00410   float realData[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00411 
00412   // check if we should switch to command mode...
00413   if(fri_msr_.intf.state == 1 && fri_msr_.intf.quality == 3
00414       && krl_time_ == -1 && fri_msr_.robot.control == 3 && !closing_)   {
00415     printf("### switching to command mode... %f\n", krl_time_);
00416     postCommand(intData, realData);
00417   }
00418 
00419   // reflecting command / monitor state to KRL
00420   fri_cmd_.krl.boolData &= 0x7f;
00421   fri_cmd_.krl.boolData |= (fri_msr_.intf.state == 2) << 7;
00422 }
00423 
00424 
00425 const char* FRIComm::printStatus(char* buffer,
00426                                  RobotData data,
00427                                  RobotCommand cmd,
00428                                  RobotStatus status)
00429 {
00430   int off=0;
00431   char qualities[] = {'x', '-', '+', '*'}; char comm_states[] = {'0', 'M', 'C'};
00432   char quality = qualities[status.quality], state = comm_states[status.state];
00433 
00434   float *p=data.position, *cd=data.commanded, *c=cmd.command;
00435 
00436   //off+=sprintf(buffer+off, "power=%x control=%x error=%x warning=%x\n",fri_msr_.robot.power, fri_msr_.robot.control, fri_msr_.robot.error, fri_msr_.robot.warning);
00437 
00438   off+=sprintf(buffer+off, "t=%7.1f    pos=(% 7.2f % 7.2f % 7.2f % 7.2f % 7.2f % 7.2f % 7.2f)\n",
00439          status.time, p[0]/M_PI*180.0, p[1] RAD, p[2] RAD,
00440          p[3] RAD, p[4] RAD, p[5] RAD, p[6] RAD);
00441   off+=sprintf(buffer+off, "l=%1.5f  cmded=(% 7.2f % 7.2f % 7.2f % 7.2f % 7.2f % 7.2f % 7.2f)\n",
00442          status.latency, cd[0] RAD, cd[1] RAD, cd[2] RAD,
00443          cd[3] RAD, cd[4] RAD, cd[5] RAD, cd[6] RAD);
00444   off+=sprintf(buffer+off, "%c%c           cmd=(% 7.2f % 7.2f % 7.2f % 7.2f % 7.2f % 7.2f % 7.2f)\n\n",
00445          state, quality, c[0] RAD, c[1] RAD, c[2] RAD, c[3] RAD, c[4] RAD, c[5] RAD, c[6] RAD);
00446 
00447   return buffer;
00448 }
00449 
00450 
00451 bool FRIComm::runstop()
00452 {
00453   return runstop_;
00454 }
00455 
00456 
00457 void FRIComm::setRunstop(bool stopped)
00458 {
00459   runstop_ = stopped;
00460 }
00461 
00462 
00463 RobotData FRIComm::data()
00464 {
00465   RobotData data;
00466 
00467   data.seq = fri_msr_.head.sendSeqCount;
00468 
00469   for(int i=0; i < 7; i++) {
00470     data.position[i] = fri_msr_.data.msrJntPos[i];
00471     //data.commanded[i] = fri_msr_.data.cmdJntPos[i] + fri_msr_.data.cmdJntPosFriOffset[i];
00472     data.commanded[i] = fri_cmd_.cmd.jntPos[i];
00473     data.torque[i] = fri_msr_.data.estExtJntTrq[i];
00474     data.torque_raw[i] = fri_msr_.data.msrJntTrq[i];
00475   }
00476 
00477   for(int i=0; i < 6; i++) {
00478     data.torqueTCP[i] = fri_msr_.data.estExtTcpFT[i];
00479   }
00480 
00481   return data;
00482 }
00483 
00484 
00485 RobotCommand FRIComm::cmd()
00486 {
00487   RobotCommand c;
00488   float *pos = safety_->pos();
00489 
00490   for(int i=0; i < 7; i++) {
00491     c.command[i]   = (fri_cmd_.cmd.jntPos[i] - pos[i]) / fri_msr_.intf.desiredCmdSampleTime;
00492     c.stiffness[i] = fri_cmd_.cmd.jntStiffness[i];
00493     c.damping[i]   = fri_cmd_.cmd.jntDamping[i];
00494     c.addTorque[i] = fri_cmd_.cmd.addJntTrq[i];
00495   }
00496 
00497   c.useCartesianImpedance = useCartesianImpedance_;
00498   c.cartImpedance = cartesianImpedanceCmd_;
00499 
00500   return c;
00501 }
00502 
00503 
00504 void FRIComm::setCmd(RobotCommand cmd)
00505 {
00506   cmd.incrementPosition(fri_cmd_.cmd.jntPos, safety_->pos(), fri_msr_.intf.desiredCmdSampleTime);
00507 
00508   for(int i=0; i < 7; i++) {
00509       fri_cmd_.cmd.jntStiffness[i] = cmd.stiffness[i];
00510       fri_cmd_.cmd.jntDamping[i] = cmd.damping[i];
00511       fri_cmd_.cmd.addJntTrq[i] = cmd.addTorque[i];
00512   }
00513 
00514   useCartesianImpedance_ = cmd.useCartesianImpedance;
00515   cartesianImpedanceCmd_ = cmd.cartImpedance;
00516 }
00517 
00518 
00519 RobotStatus FRIComm::status()
00520 {
00521   RobotStatus s;
00522   s.time = fri_msr_.intf.timestamp;
00523   s.answerRate = fri_msr_.intf.stat.answerRate;
00524   s.latency = fri_msr_.intf.stat.latency;
00525   s.jitter = fri_msr_.intf.stat.jitter;
00526   s.missRate = fri_msr_.intf.stat.missRate;
00527   s.missCounter = fri_msr_.intf.stat.missCounter;
00528 
00529   s.state = fri_msr_.intf.state;
00530   s.quality = fri_msr_.intf.quality;
00531   s.msrSampleTime = fri_msr_.intf.desiredMsrSampleTime;
00532   s.cmdSampleTime = fri_msr_.intf.desiredCmdSampleTime;
00533   s.safety = fri_msr_.intf.safetyLimits;
00534 
00535   s.power = fri_msr_.robot.power;
00536   s.controlMode = fri_msr_.robot.control;
00537   s.error = fri_msr_.robot.error;
00538   s.warning = fri_msr_.robot.warning;
00539   for(int i=0; i < 7; i++)
00540     s.temperature[i] = fri_msr_.robot.temperature[i];
00541 
00542   s.runstop = runstop_;
00543 
00544   // internal timing stats
00545   if(fri_msr_.intf.desiredCmdSampleTime != 0 && duration_num != 0)
00546   {
00547     int interval = 1.0 / fri_msr_.intf.desiredCmdSampleTime;
00548     if(duration_num >= interval)
00549     {
00550       duration_mean_published = duration_sum / duration_num;
00551       duration_max_published = duration_max;
00552       duration_num = 0;
00553       duration_sum = 0;
00554       duration_max = 0;
00555     }
00556   }
00557 
00558   s.calcTimeMax  = duration_max_published;
00559   s.calcTimeMean = duration_mean_published;
00560 
00561   return s;
00562 }
00563 
00564 
00565 void FRIComm::tick()
00566 {
00567   gettimeofday(&last_tick, 0);
00568 }
00569 
00570 void FRIComm::tock()
00571 {
00572   struct timeval now;
00573   gettimeofday(&now, 0);
00574 
00575   int duration = 1e6*(now.tv_sec  - last_tick.tv_sec)
00576                            + (now.tv_usec - last_tick.tv_usec);
00577 
00578   duration_sum += duration;
00579   duration_max  = (duration > duration_max) ? duration : duration_max;
00580   duration_num++;
00581 }
00582 
00583 
00584 
00585 FRIThread::FRIThread() :
00586   exitRequested_(false), runstop_buffer_(false)
00587 {
00588   krlcmd_buffer_.fresh = false;
00589   for(int i=0; i < 16; i++) {
00590     krlcmd_buffer_.iData[i] = 0;
00591     krlcmd_buffer_.rData[i] = 0.0;
00592   }
00593 }
00594 
00595 
00596 void FRIThread::configureNetwork(int local_port, const char* remote_address, int remote_port)
00597 {
00598   fri.configureNetwork(local_port, remote_address, remote_port);
00599 
00600   //DEBUGGING
00601   char buff[1024];
00602   time_t t = time(0);
00603   struct tm *now = localtime(&t);
00604 
00605   sprintf(buff, "/tmp/movelog_%s_%4d%02d%02d%02d%02d%02d.log",
00606     (local_port==7001) ? "left" : "right",
00607     1900+now->tm_year, now->tm_mon, now->tm_mday,
00608     now->tm_hour, now->tm_min, now->tm_sec);
00609 
00610   LOG=fopen(buff, "w+");
00611 }
00612 
00613 
00614 FRICheck* FRIThread::limitChecker()
00615 {
00616   return fri.limitChecker();
00617 }
00618 
00619 bool FRIThread::start()
00620 {
00621   // fri.open() does not block - we can do it here already.
00622   if(!fri.open())
00623     return false;
00624 
00625   // setting up mutex
00626   pthread_mutexattr_t mattr;
00627   pthread_mutexattr_init(&mattr);
00628   pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
00629 
00630   pthread_mutex_init(&mutex_,  &mattr);
00631 
00632   // setting up thread
00633   pthread_attr_t tattr;
00634   struct sched_param sparam;
00635   sparam.sched_priority = 12;
00636   pthread_attr_init(&tattr);
00637   pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
00638   pthread_attr_setschedparam(&tattr, &sparam);
00639   pthread_attr_setinheritsched (&tattr, PTHREAD_EXPLICIT_SCHED);
00640 
00641   if(pthread_create(&thread_, &tattr, &FRIThread::run_s, (void *) this) != 0) {
00642     printf("# ERROR: could not create realtime thread\n");
00643     return false;
00644   }
00645 
00646   running_ = true;
00647   return true;
00648 }
00649 
00650 bool FRIThread::runstop()
00651 {
00652   pthread_scoped_lock lock(&mutex_);
00653   return runstop_buffer_;
00654 }
00655 
00656 void FRIThread::setRunstop(bool stopped)
00657 {
00658   pthread_scoped_lock lock(&mutex_);
00659   runstop_buffer_ = stopped;
00660 }
00661 
00662 
00663 void* FRIThread::run()
00664 {
00665   bool runstop = false;
00666   RobotCommand cmd;
00667   RobotData data;
00668   RobotStatus status = fri.status();
00669 
00670   KRLCmd krlcmd = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
00671                    {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, false};
00672 
00673   printf("# waiting for LWR connection\n");
00674 
00675   bool communicating = false;
00676 
00677   while(!exitRequested_) {
00678     // retrieve new commands from buffer
00679     //TODO: -> trylock
00680     pthread_mutex_lock(&mutex_);
00681     runstop = runstop_buffer_;
00682     cmd = cmd_buffer_;
00683     if(krlcmd_buffer_.fresh) {
00684       krlcmd = krlcmd_buffer_;
00685       krlcmd_buffer_.fresh = false;
00686     }
00687     pthread_mutex_unlock(&mutex_);
00688 
00689     if(!fri.receive()) { // blocks for up to 0.1s
00690       communicating = false;
00691       continue;
00692     } else {
00693       communicating = true;
00694     }
00695 
00696     data = fri.data();
00697     status = fri.status();
00698     fri.setRunstop(runstop);
00699     fri.setCmd(cmd);
00700 
00701     if(krlcmd.fresh)
00702       if(fri.postCommand(krlcmd.iData, krlcmd.rData))
00703         krlcmd.fresh = false;
00704 
00705     fri.respond();
00706       
00707     // note: the data is not stored in a queue.
00708     // read fast, or you will lose data...
00709     pthread_mutex_lock(&mutex_);
00710     data_buffer_ = data;
00711     status_buffer_ = status;
00712     status_buffer_.connected = communicating;
00713     pthread_mutex_unlock(&mutex_);
00714   }
00715 
00716   printf("# exiting loop\n");
00717   fri.close();
00718   printf("# communication finished\n");
00719 
00720   running_ = false;
00721   return 0;
00722 }
00723 
00724 
00725 RobotData FRIThread::data()
00726 {
00727   pthread_scoped_lock lock(&mutex_);
00728   return data_buffer_;
00729 }
00730 
00731 bool FRIThread::running()
00732 {
00733   pthread_scoped_lock lock(&mutex_);
00734   return running_;
00735 }
00736 
00737 void FRIThread::finish()
00738 {
00739   pthread_mutex_lock(&mutex_);
00740   exitRequested_ = true;
00741   pthread_mutex_unlock(&mutex_);
00742 
00743   pthread_join(thread_, 0);
00744 }
00745 
00746 RobotCommand FRIThread::cmd()
00747 {
00748   pthread_scoped_lock lock(&mutex_);
00749   return cmd_buffer_;
00750 }
00751 
00752 RobotStatus FRIThread::status()
00753 {
00754   pthread_scoped_lock lock(&mutex_);
00755   return status_buffer_;
00756 }
00757 
00758 
00759 void FRIThread::setCmd(RobotCommand cmd)
00760 {
00761   pthread_scoped_lock lock(&mutex_);
00762   cmd_buffer_ = cmd;
00763 }
00764 
00765 
00766 void FRIThread::postCommand(int iData[16], float rData[16])
00767 {
00768   pthread_scoped_lock lock(&mutex_);
00769   for(int i=0; i < 16; i++) {
00770     krlcmd_buffer_.iData[i] = iData[i];
00771     krlcmd_buffer_.rData[i] = rData[i];
00772     krlcmd_buffer_.fresh = true;
00773   }
00774 }


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Mon Oct 6 2014 09:27:39