Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WorldStatePool.cc

Go to the documentation of this file.
00001 #include "WorldStatePool.h"
00002 #include "Shared/MarkScope.h"
00003 #include "Shared/debuget.h"
00004 #include "Shared/Config.h"
00005 #include "IPC/RCRegion.h"
00006 #ifndef PLATFORM_APERIOS
00007 #  include "Events/EventRouter.h"
00008 #  include "local/LoadDataThread.h"
00009 #endif
00010 
00011 //better to put this here instead of the header
00012 using namespace std; 
00013 
00014 WorldStatePool::WorldStatePool() : order(), lock() {
00015   //initialize order with 1,2,3...NUM_STATES
00016   while(order.size()<order.getMaxCapacity()) {
00017     unsigned int i=order.size();
00018     reading[i]=writing[i]=frames[i]=0;
00019     status[i]=0;
00020 #ifdef PLATFORM_APERIOS
00021     stateLookupMap[i]=NULL;
00022 #else
00023     complete[i]=MessageQueueBase::getSemaphoreManager()->getSemaphore();
00024 #endif
00025     order.push_back(i);
00026   }
00027 #ifdef PLATFORM_APERIOS
00028   complete[order.size()-1].lock(ProcessID::getID());
00029 #else
00030   MessageQueueBase::getSemaphoreManager()->setValue(complete[order.size()-1],1);
00031 #endif
00032 }
00033 
00034 void WorldStatePool::doUseResource(Data& d) {
00035   Request& r=static_cast<Request&>(d);
00036   if(r.isRead) {
00037     ReadRequest& rd = static_cast<ReadRequest&>(d);
00038     if(rd.depth++==0) {
00039       rd.prev=rd.tgt;
00040       rd.bufUsed=getCurrentReadState(rd.tgt,rd.bl);
00041       //cout << ProcessID::getIDStr() << " reading " << rd.bufUsed << " (" << state << ")" << endl;
00042     }
00043   } else {
00044     WriteRequest& write = static_cast<WriteRequest&>(d);
00045     if(write.depth++==0) {
00046       useResource(write.srcRequest);
00047       write.prev=write.tgt;
00048       write.bufUsed=getCurrentWriteState(write.tgt,write.frame,write.bl);
00049       if(write.bufUsed!=-1U) {
00050         write.setStatus(status[write.bufUsed]);
00051         write.setComplete(isComplete(write.bufUsed));
00052         //cout << ProcessID::getIDStr() << " writing " << write.bufUsed << " (" << state << ")" << endl;
00053       }
00054     }
00055   }
00056 #ifdef PLATFORM_APERIOS
00057   stateLookupMap[ProcessID::getID()]=state;
00058 #endif
00059 }
00060 void WorldStatePool::doReleaseResource(Data& d) {
00061   Request& r=static_cast<Request&>(d);
00062   if(r.isRead) {
00063     ReadRequest& rd = static_cast<ReadRequest&>(d);
00064     if(rd.depth==0)
00065       throw std::underflow_error("WorldStatePool read resource usage underflow");
00066     if(--rd.depth==0) {
00067       if(rd.bufUsed!=-1U) {
00068         doneReadingState(rd.bufUsed);
00069         rd.bufUsed=-1U;
00070       }
00071       rd.tgt=rd.prev;
00072     }
00073   } else {
00074     WriteRequest& write = static_cast<WriteRequest&>(d);
00075     if(write.depth==0)
00076       throw std::underflow_error("WorldStatePool write resource usage underflow");
00077     if(--write.depth==0) {
00078       if(write.bufUsed!=-1U) {
00079         status[write.bufUsed]=write.getStatus();
00080         doneWritingState(write.bufUsed,write.getComplete());
00081         write.bufUsed=-1U;
00082       }
00083       write.tgt=write.prev;
00084       releaseResource(write.srcRequest);
00085     }
00086   }
00087 #ifdef PLATFORM_APERIOS
00088   stateLookupMap[ProcessID::getID()]=state;
00089 #endif
00090 }
00091 
00092 #ifdef PLATFORM_APERIOS
00093 
00094 WorldStatePool::UpdateInfo* WorldStatePool::isUnread(OSensorFrameVectorData& msg, unsigned int& lastFrameNumber) {
00095   //cout << ProcessID::getIDStr() << " checking tgtFr=" << msg.GetInfo(0)->frameNumber << " lastFr=" << lastFrameNumber << endl;
00096   
00097   static UpdateInfo info; // avoiding heap allocation by using static here, but this means not threadsafe (shouldn't be a problem...)
00098   info.msg=&msg;
00099   info.frameNumber=msg.GetInfo(0)->frameNumber;
00100   unsigned int toUse=selectWriteState(info.frameNumber,true);
00101 #ifdef DEBUG
00102   info.intendedBuf=toUse;
00103 #endif
00104   if(toUse==-1U)
00105     return NULL; //error occurred, should already be reported
00106   //cout << ProcessID::getIDStr() << " Intending to use " << toUse << " holding frame " << frames[toUse] << " marked " << (isComplete(toUse)?"complete":"not complete") << " with status " << status[toUse] << " for frame " << info.frameNumber << endl;
00107   if(frames[toUse]==info.frameNumber && isComplete(toUse)) {
00108     if(info.frameNumber-lastFrameNumber!=NumFrames)
00109       cout << ProcessID::getIDStr() << " dropped " << (info.frameNumber-lastFrameNumber-NumFrames)/NumFrames << " sensor frame(s)" << endl;
00110     lastFrameNumber=info.frameNumber;
00111     //cout << ProcessID::getIDStr() << " decided frame is complete or nothing to contribute" << endl;
00112     return NULL;
00113   }
00114   return &info;
00115 }
00116 
00117 #else //PLATFORM_LOCAL
00118 
00119 WorldStatePool::UpdateInfo* WorldStatePool::isUnread(const RCRegion& msg, unsigned int curFrameNumber, unsigned int& lastFrameNumber, bool /*haveFeedback*/, bool /*motionOverride*/) {
00120   //cout << ProcessID::getIDStr() << " checking tgtFr=" << curFrameNumber << " lastFr=" << lastFrameNumber << " fdbkAv=" << haveFeedback << " ovrd="<<motionOverride << endl;
00121   
00122   static UpdateInfo info; // avoiding heap allocation by using static here, but this means not threadsafe (shouldn't be a problem...)
00123   info.payload=LoadDataThread::deserializeHeader(msg.Base(),msg.Size(),&info.verbose,&info.frameNumber,&info.filename,&info.dataInQueue,&info.payloadSize);
00124   if(info.payload==NULL)
00125     throw std::runtime_error("deserialization of sensor update header failed");
00126   if(info.frameNumber<curFrameNumber) {
00127     // this means there's a newer one already in the queue -- skip this (and we'll report the drop next time -- no assignment to lastFrameNumber)
00128     return NULL;
00129   }
00130   if(info.payloadSize==0)
00131     info.payload=NULL;
00132   
00133   unsigned int toUse=selectWriteState(info.frameNumber,true);
00134 #ifdef DEBUG
00135   info.intendedBuf=toUse;
00136 #endif
00137   if(toUse==-1U)
00138     return NULL; //error occurred, should already be reported
00139   if(info.verbose>=3 && info.payloadSize==0)
00140     cout << ProcessID::getIDStr() << " received sensor heartbeat at " << get_time() << endl;
00141   else if(info.verbose>=2 && info.payloadSize!=0)
00142     cout << ProcessID::getIDStr() << " received sensor data \"" << info.filename << "\" at " << get_time() << endl;
00143   //cout << "Intending to use " << toUse << " holding frame " << frames[toUse] << " marked " << (isComplete(toUse)?"complete":"not complete") << " with status " << status[toUse] << " for frame " << info.frameNumber << endl;
00144   if(frames[toUse]==info.frameNumber && isComplete(toUse)) {
00145     if(info.frameNumber-lastFrameNumber!=1 && info.verbose>=1)
00146       cout << ProcessID::getIDStr() << " dropped " << (info.frameNumber-lastFrameNumber-1) << " sensor frame(s)" << endl;
00147     lastFrameNumber=info.frameNumber;
00148     //cout << ProcessID::getIDStr() << " decided frame is complete or nothing to contribute" << endl;
00149     return NULL;
00150   }
00151   return &info;
00152 }
00153 
00154 bool WorldStatePool::read(const WorldStatePool::UpdateInfo& info, WriteRequest& wsw, bool feedbackGenerated, bool zeroPIDFeedback, const PostureEngine* feedback/*=NULL*/) {
00155   //cout << ProcessID::getIDStr() << " writing " << wsw.frame << " state=" << state << "(" << wsw.bufUsed << ") source=" << wsw.src << "("<<wsw.srcRequest.bufUsed<<") status="<<wsw.getStatus() << " complete="<<wsw.getComplete() << endl;
00156   
00157   ASSERT(feedbackGenerated || feedback==NULL, "feedbackGenerated is false, yet feedback was supplied?");
00158   if(wsw.getComplete()) {
00159     // this could happen if both processes were trying to get the write lock at the same time
00160     // and the first in filled everything by itself (i.e. no feedback required, or the one with feedback was there first)
00161     return true;
00162   } 
00163   ASSERT((wsw.getStatus()&FEEDBACK_APPLIED)==0, "feedback applied, but apparently not completed "<<wsw.getComplete());
00164   //ASSERT(wsw.bufUsed==info.intendedBuf,"read() not using expected state buffer"); //not a problem (I think? just fell an even number of frames behind the other process?)
00165   
00166   if((wsw.getStatus()&SENSORS_APPLIED) != 0) {
00167     // sensors already applied, just need to do feedback, if any
00168     if(feedback!=NULL) {
00169       // have feedback, copy it in
00170       if(zeroPIDFeedback) {
00171         // copy all values, regardless of PID values
00172         for(unsigned int i=0; i<NumOutputs; i++)
00173           state->outputs[i]=feedback->getOutputCmd(i).value;
00174       } else {
00175         // only copy non-zero PID values
00176         for(unsigned int i=0; i<NumOutputs; i++)
00177           if(!isZeroPID(wsw.src,i))
00178             state->outputs[i]=feedback->getOutputCmd(i).value;
00179       }
00180       //Apply calibration to the joints (this reverses the calibration from MotionManager
00181       for (uint i=0; i<NumPIDJoints; i++)
00182         state->outputs[i+PIDJointOffset] = state->outputs[i+PIDJointOffset] / config->motion.calibration_scale[i]
00183           - config->motion.calibration_offset[i];
00184       wsw.setStatus(wsw.getStatus() | FEEDBACK_APPLIED);
00185     }
00186   
00187   } else {
00188     // need to do sensors (if present), as well as feedback (if any)
00189     static PostureEngine pose; //this is already thread-unsafe, might as well use another static tmp here to avoid initialization cost
00190     if(info.payload==NULL) {
00191       
00192       // no new sensors, but still have to copy over old values (downside of having multiple state buffers)
00193       pose.setLoadedSensors(wsw.src);
00194       if(!feedbackGenerated) {
00195         // no feedback either, take all of the old positions
00196         pose.takeSnapshot(*wsw.src);
00197         pose.setWeights(1);
00198       } else if(zeroPIDFeedback) {
00199         // feedback is applied, regardless of PID settings
00200         if(feedback!=NULL) {
00201           for(unsigned int i=0; i<NumPIDJoints; ++i)
00202             pose(i+PIDJointOffset)=feedback->getOutputCmd(i).value;
00203           wsw.setStatus(wsw.getStatus() | FEEDBACK_APPLIED);
00204         } //else wait for process with feedback to fill it in
00205       } else {
00206         // feedback is only applied to non-zero PIDs, the rest need to take the previous value
00207         if(feedback!=NULL) {
00208           // this process does have the feedback, fill in everything 
00209           for(unsigned int i=0; i<NumPIDJoints; ++i)
00210             if(isZeroPID(wsw.src,i))
00211               pose(i+PIDJointOffset)=wsw.src->outputs[i+PIDJointOffset];
00212             else
00213               pose(i+PIDJointOffset)=feedback->getOutputCmd(i).value;
00214           wsw.setStatus(wsw.getStatus() | FEEDBACK_APPLIED);
00215         } else {
00216           // this process doesn't have the feedback, only fill in the non-feedback
00217           for(unsigned int i=0; i<NumPIDJoints; ++i)
00218             if(isZeroPID(wsw.src,i))
00219               pose(i+PIDJointOffset)=wsw.src->outputs[i+PIDJointOffset];
00220         }
00221       }
00222       
00223     } else {
00224       
00225       unsigned int stateFrame=state->frameNumber; //back up current frame number so we can restore it after the load 
00226       // new sensor values are provided
00227       pose.setLoadedSensors(state);
00228       //cout << "Parsing sensor data " << info.payloadSize << " bytes" << endl;
00229       //cout << string(info.payload,info.payloadSize) << endl;
00230       size_t x=pose.loadBuffer(info.payload,info.payloadSize);
00231       //cout << "read " << x << endl;
00232       if(!x) {
00233         cerr << "ERROR: Corrupted sensor readings received by Main" << endl;
00234         return false;
00235       }
00236       state->frameNumber=stateFrame; // restore the frame number over what was loaded from the file so state->read doesn't unnecessarily skip it below
00237       
00238       if(stateFrame>wsw.src->frameNumber)
00239         return false; // I'm not sure how this occurs, but sometimes (if doing a lot of paging/kernel stuff that screws up the scheduler) the source is newer than our update
00240       ASSERT(stateFrame<wsw.src->frameNumber || stateFrame==1 && wsw.src->frameNumber==1,"already updated (" << stateFrame << " vs " << wsw.src->frameNumber << ")?  So why did " << ProcessID::getIDStr() << " parse...");
00241       
00242       if(!feedbackGenerated) {
00243         // no feedback, so just use all of the values we just loaded
00244         // weight may be 0 if sensor which sent update doesn't provide joint positions
00245         //for(unsigned int i=0; i<NumOutputs; ++i)
00246         //ASSERT(pose(i).weight>0,"zero weight found after loading"); // one would expect the compiler to no-op this loop ifndef DEBUG
00247       } else if(zeroPIDFeedback) {
00248         // feedback is applied, regardless of PID settings
00249         if(feedback!=NULL) {
00250           for(unsigned int i=0; i<NumPIDJoints; ++i)
00251             pose(i+PIDJointOffset)=feedback->getOutputCmd(i).value;
00252           wsw.setStatus(wsw.getStatus() | FEEDBACK_APPLIED);
00253         } //else wait for process with feedback to fill it in
00254       } else {
00255         // feedback is only applied to non-zero PIDs, the rest need to take the previous value
00256         if(feedback!=NULL) {
00257           // this process does have the feedback, fill in everything 
00258           for(unsigned int i=0; i<NumPIDJoints; ++i)
00259             if(!isZeroPID(wsw.src,i))
00260               pose(i+PIDJointOffset)=feedback->getOutputCmd(i).value;
00261             /*else // this can happen validly if starting up with zero PID and unknown positions
00262               ASSERT(pose(i+PIDJointOffset).weight!=0,"zero weight found after loading");*/
00263           wsw.setStatus(wsw.getStatus() | FEEDBACK_APPLIED);
00264         } else {
00265           // this process doesn't have the feedback, only fill in the non-feedback
00266           for(unsigned int i=0; i<NumPIDJoints; ++i)
00267             if(!isZeroPID(wsw.src,i))
00268               pose(i+PIDJointOffset).weight=0; // don't apply sensor values to non-zero pids (this will come from feedback later)
00269             /*else // this can happen validly if starting up with zero PID and unknown positions
00270               ASSERT(pose(i+PIDJointOffset).weight!=0,"zero weight found after loading");*/
00271         }
00272       }
00273     }
00274     state->read(pose,wsw.src,erouter);
00275     //ASSERT((state->frameNumber-5)/4==wsw.frame,"state frame number and message serial number are desynchronized " << state->frameNumber << " vs " << wsw.frame);
00276     state->frameNumber=(wsw.frame+1)*NumFrames+1; //lets dropped messages be skipped
00277     wsw.setStatus(wsw.getStatus() | SENSORS_APPLIED);
00278   }
00279   wsw.setComplete(feedbackGenerated && wsw.getStatus()==(SENSORS_APPLIED|FEEDBACK_APPLIED) || !feedbackGenerated && (wsw.getStatus()&SENSORS_APPLIED)!=0);
00280   //cout << ProcessID::getIDStr() << " fr=" << info.frameNumber << " pyld=" << info.payloadSize << " fdbkG=" << feedbackGenerated << " pidfdbk=" << zeroPIDFeedback << " fdbk=" << feedback << " set status="<<wsw.getStatus() << " complete=" << wsw.getComplete() << endl;
00281   return true;
00282 }
00283 
00284 #endif
00285 
00286 
00287 unsigned int WorldStatePool::getCurrentReadState(WorldState*& tgt, bool block) {
00288   unsigned int toUse=-1U;
00289   if(block) {
00290     while(toUse!=order.back()) {
00291       {
00292         MarkScope l(lock);
00293         if(toUse!=-1U) { //newer frame added while we were waiting
00294           //std::cerr << ProcessID::getID() << " WARNING WorldStatePool: whoa new frame while blocking read " << toUse << " vs " << order.back() << endl;
00295           reading[toUse]--;
00296         }
00297         toUse=order.back();
00298         reading[toUse]++;
00299       } //release WorldStatePool lock to allow new readers to come through
00300 #ifdef PLATFORM_APERIOS
00301       MarkScope wl(complete[toUse]); //now block until writing lock is released
00302 #else
00303       MessageQueueBase::getSemaphoreManager()->testZero(complete[toUse],true); //block until complete semaphore is released (set to 0)
00304 #endif
00305     }
00306   } else {
00307     MarkScope l(lock);
00308     order_t::index_t idx;
00309     for(idx=order.prev(order.end()); idx!=order.end(); idx=order.prev(idx)) {
00310       toUse=order[idx];
00311       if(isComplete(toUse))
00312         break;
00313     }
00314     if(idx==order.end()) {
00315       std::cerr << "ERROR: WorldStatePool unable to read state because none available" << std::endl;
00316       //return -1U;
00317       std::cerr << "       falling into most recent buffer" << std::endl;
00318       toUse=order.prev(order.end());
00319     }
00320     reading[toUse]++;
00321   }
00322   //std::cout << ProcessID::getID() << " reading " << toUse << endl;
00323   //ASSERTRETVAL(toUse!=-1U,"toUse was not chosen!",-1U);
00324 #ifdef DEBUG
00325   if(toUse==-1U) {
00326     std::cerr << "ERROR: WorldStatePool: toUse was not chosen!" << std::endl;
00327     //return -1U;
00328     std::cerr << "       falling into most recent buffer" << std::endl;
00329     toUse=order.prev(order.end());
00330     reading[toUse]++;
00331   }
00332 #endif
00333   tgt=&states[toUse];
00334   return toUse;
00335 }
00336 void WorldStatePool::doneReadingState(unsigned int i) {
00337   MarkScope l(lock);
00338   reading[i]--;
00339   //std::cout << ProcessID::getID() << " reading " << i << " - done" << endl;
00340 }
00341 
00342 bool WorldStatePool::isComplete(unsigned int idx) const {
00343 #ifdef PLATFORM_APERIOS
00344   return static_cast<unsigned int>(complete[idx].owner())==MutexLockBase::NO_OWNER;
00345 #else
00346   return MessageQueueBase::getSemaphoreManager()->testZero(complete[idx],false);
00347 #endif
00348 }
00349 
00350 unsigned int WorldStatePool::selectWriteState(unsigned int frame, bool block, order_t::index_t& idx) const {
00351   //check the end to verify a newer (or equivalent) update isn't already in progress
00352   idx=order.prev(order.end());
00353   unsigned int toUse=order[idx];
00354   if(frames[toUse]>frame) {
00355     // can happen if one thread (say MotionExec running a motion update) takes too long, another thread (say motion gotSensors updater) blocks on it, is delayed too long
00356     // we'll leave it up to the client to detect and display an appropriate message if dropping updates is a problem.
00357     //std::cerr << "WARNING: WorldStatePool found a newer write already in progress (writing " << frame << ", found " << frames[toUse] << ")" << std::endl;
00358     return -1U;
00359   }
00360   if(frames[toUse]!=frame || !block) {
00361     //start at the beginning and run forward to get the oldest unreferenced entry
00362     order_t::index_t fallback=order.end();
00363     for(idx=order.begin(); idx!=order.end(); idx=order.next(idx)) {
00364       toUse=order[idx];
00365       if(frames[toUse]>=frame) {
00366         idx=order.end();
00367         break;
00368       }
00369       if(writing[toUse]==0) { //note: could be incomplete -- if this is the oldest frame and it's not currently being written to, but yet wasn't completed, we'll recycle it
00370         if(reading[toUse]==0)
00371           break; //found our entry!
00372       } else if(fallback==order.end())
00373         fallback=idx; //in case we don't find an unreferenced one later in the list
00374     }
00375     if(idx==order.end()) {
00376       if(block && fallback!=order.end())
00377         toUse=order[idx=fallback];
00378       else {
00379         std::cerr << "ERROR: WorldStatePool unable to update state because none available" << std::endl;
00380         return -1U;
00381       }
00382     }
00383   }
00384   return toUse;
00385 }
00386 
00387 unsigned int WorldStatePool::getCurrentWriteState(WorldState*& tgt, unsigned int frame, bool block) {
00388   unsigned int toUse;
00389   { //artificial scope to limit lock on next line
00390     MarkScope l(lock);
00391     order_t::index_t idx;
00392     toUse=selectWriteState(frame,block,idx);
00393     if(toUse==-1U)
00394       return -1U;
00395     //std::cout << ProcessID::getID() << " writing " << toUse << " (frame " << frame << ")" << endl;
00396     //ASSERTRETVAL(toUse!=-1U,"toUse was not chosen!",-1U);
00397     writing[toUse]++;
00398     if(frames[toUse]!=frame) {
00399       //move this entry to the back of the list, keep frames sorted (in order of write initiation / frame number)
00400       order.erase(idx);
00401       order.push_back(toUse);
00402       frames[toUse]=frame;
00403       status[toUse]=0;
00404 #ifdef PLATFORM_APERIOS
00405       complete[toUse].releaseAll(); //make sure it wasn't an incomplete frame that's being recycled
00406       complete[toUse].lock(ProcessID::getID());
00407 #else
00408       MessageQueueBase::getSemaphoreManager()->setValue(complete[toUse],1);
00409 #endif
00410     }
00411   }
00412   if(block) {
00413     writeLocks[toUse].lock(ProcessID::getID());
00414   } else if(!writeLocks[toUse].try_lock(ProcessID::getID())) {
00415     std::cerr << "WARNING: WorldStatePool unable to acquire write lock (blocking reader?).  Trying again..." << std::endl;
00416     writeLocks[toUse].lock(ProcessID::getID());
00417   }
00418   if(toUse!=order.back()) { //newer frame added while we were waiting
00419     // This is a very rare case
00420     //std::cerr << ProcessID::getID() << " WARNING WorldStatePool: whoa new frame while blocking write " << toUse << " vs " << order.back() << endl;
00421     // go back to square one
00422     doneWritingState(toUse,false);
00423     return getCurrentWriteState(tgt,frame,block);
00424   }
00425   tgt=&states[toUse];
00426   return toUse;
00427 }
00428 void WorldStatePool::doneWritingState(unsigned int i, bool completed) {
00429   MarkScope l(lock);
00430   //std::cout << ProcessID::getID() << " writing " << i << " - done" << endl;
00431   writing[i]--;
00432   if(completed) {
00433 #ifdef PLATFORM_APERIOS
00434     if(!isComplete(i))
00435       complete[i].unlock();
00436 #else
00437     MessageQueueBase::getSemaphoreManager()->setValue(complete[i],0);
00438 #endif
00439   }
00440   writeLocks[i].unlock();
00441 }
00442 
00443 
00444 
00445 /*! @file
00446  * @brief 
00447  * @author Ethan Tira-Thompson (ejt) (Creator)
00448  *
00449  * $Author: kcomer $
00450  * $Name: tekkotsu-4_0 $
00451  * $Revision: 1.30 $
00452  * $State: Exp $
00453  * $Date: 2007/11/15 21:33:04 $
00454  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:57 2007 by Doxygen 1.5.4