Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

LoadDataThread.cc

Go to the documentation of this file.
00001 #include "LoadDataThread.h"
00002 #include "Shared/get_time.h"
00003 #include "Shared/debuget.h"
00004 #include <sys/stat.h>
00005 #include <regex.h>
00006 #include <dirent.h>
00007 #include <set>
00008 #include <errno.h>
00009 
00010 using namespace std;
00011 
00012 // allocated here to avoid creating otherwise empty DataSource.cc
00013 unsigned int * DataSource::providedOutputs=NULL;
00014 bool DataSource::requiresFirstSensor=false;
00015 
00016 LoadDataThread::~LoadDataThread() {
00017   MarkScope l(lock);
00018   DeviceDriver::getRegistry().removeCollectionListener(this);
00019   framerate.removePrimitiveListener(this);
00020   verbose.removePrimitiveListener(this);
00021   frozen.removePrimitiveListener(this);
00022   while(regions.size()>0) {
00023     freeRegion(regions.front());
00024     regions.pop_front();
00025   }
00026 }
00027 
00028 bool LoadDataThread::advanceFrame(bool forceQueue) {
00029   MarkScope l(lock);
00030   if(frozen && !forceQueue || curDS==NULL) {
00031     if(heartbeat)
00032       sendHeartbeat();
00033     return false;
00034   }
00035   
00036   // check to see if runloop has a frame in progress -- send that one ASAP, don't cut line
00037   if(runloopState==GET_FRAME) {
00038     runloopState=ADVANCE_FRAME;
00039     return true;
00040   } else if(runloopState==GOT_FRAME) {
00041     runloopState=ADVANCE_FRAME;
00042     interrupt(); //break out of sleep
00043     return true;
00044   }
00045   
00046   //! safe to force the next frame ourselves
00047   const char* payload;
00048   unsigned int sz;
00049   unsigned int advTime = get_time();
00050   unsigned int timestamp = (forceQueue ? 0 : advTime);
00051   std::string name;
00052   unsigned int sn=curDS->getData(payload, sz, timestamp, name);
00053   if(payload==NULL) {
00054     if(heartbeat)
00055       sendHeartbeat();
00056     return false;
00057   } else {
00058     if(verbose>=1 && sn!=sourceSN+1 && sourceSN!=0)
00059       std::cout << "Data Source dropped " << (sn-sourceSN-1) << " frames"  << std::endl;
00060     sourceSN=sn;
00061   }
00062   if(verbose>=2)
00063     std::cout << "Advancing frame: \"" << name << "\" (advanced at " << advTime << ')' << std::endl;
00064   RCRegion * r=firstUnusedRegion();
00065   char * buf=setupRegion(r,name,sz,curDS->nextTimestamp()!=-1U);
00066   ASSERTRETVAL(buf!=NULL,"setupRegion returned null region",false);
00067   memcpy(buf,payload,sz);
00068   msgr.sendMessage(r);
00069   regions.push_back(r);
00070   lastSent=advTime;
00071   return true;
00072 }
00073 
00074 void LoadDataThread::plistValueChanged(const plist::PrimitiveBase& pl) {
00075   MarkScope l(lock);
00076   if(&pl==&source) {
00077     std::string::size_type dot=source.find('.');
00078     DeviceDriver* dd=DeviceDriver::getRegistry().getInstance(source.substr(0,dot));
00079     if(dd==NULL) {
00080       if(source.size()!=0)
00081         std::cerr << "Could not find driver named '" << source << "' for LoadDataThread" << std::endl;
00082       setDataSource(dd, NULL);
00083     } else {
00084       std::map<std::string,DataSource*> dsmap;
00085       (dd->*getDataSources)(dsmap);
00086       if(dsmap.size()==0)
00087         setDataSource(dd, NULL);
00088       else if(dot==std::string::npos)
00089         setDataSource(dd, dsmap.begin()->second);
00090       else {
00091         std::map<std::string,DataSource*>::const_iterator ds=dsmap.find(source.substr(dot+1));
00092         setDataSource(dd, ds==dsmap.end() ? NULL : ds->second);
00093       }
00094     }
00095   } else if(&pl==&framerate) {
00096     if(curDS!=NULL)
00097       curDS->setDataSourceFramerate(framerate);
00098   } else if(&pl==&verbose) {
00099     msgr.setReportDroppings(verbose>=1);
00100     if(curDS!=NULL)
00101       curDS->setDataSourceVerbose(verbose);
00102   } else if(&pl==&frozen) {
00103     if(curDS!=NULL)
00104       curDS->setFrozen(frozen);
00105     if(enabled) {
00106       if(!frozen) {
00107         start();
00108       } else if(isStarted() && !heartbeat) {
00109         stop();
00110         enabled=true; // retain enabled setting
00111       }
00112     }
00113   } else if(&pl==&heartbeat) {
00114     if(enabled) {
00115       if(heartbeat) {
00116         start();
00117       } else if(isStarted() && frozen) {
00118         stop();
00119         enabled=true; // retain enabled setting
00120       }
00121     }
00122   } else {
00123     cerr << "LoadDataThread didn't handle call to plistValueChanged for " << pl.get() << endl;
00124   }
00125 }
00126 
00127 void LoadDataThread::start() {
00128   enabled=true;
00129   if(!isStarted() && getTimeScale()>0 && (!frozen || heartbeat) )
00130     PollThread::start();
00131 }
00132 void LoadDataThread::stop() {
00133   enabled=false;
00134   runloopState=INTERFRAME;
00135   PollThread::stop();
00136 }
00137 
00138 RCRegion* LoadDataThread::firstUnusedRegion() {
00139   for(msgbuf_t::iterator it=regions.begin();it!=regions.end(); ++it) {
00140     if((*it)->NumberOfReference()==1) {
00141       RCRegion * ans=*it;
00142       regions.erase(it);
00143       return ans;
00144     }
00145   }
00146   return NULL;
00147 }
00148 
00149 char* LoadDataThread::setupRegion(RCRegion*& region, const std::string& filename, unsigned int payload, bool hasMoreData) {
00150   // header fields: bool:verbose, uint:frameSN, str:filename, uint:payload
00151   const unsigned int FILE_HEADER_SIZE=LoadSave::getSerializedSize<unsigned int>() + LoadSave::getSerializedSize(frameSN) + LoadSave::getSerializedSize(filename) + LoadSave::getSerializedSize<bool>() + LoadSave::getSerializedSize(payload);
00152   if(region==NULL)
00153     region=new RCRegion(FILE_HEADER_SIZE+payload+sizeof(char)*filename.size()*2); //triple the allocation for filename so we don't have to resize if we get a longer name later
00154   else if(region->Size()<FILE_HEADER_SIZE+payload) {
00155     //too small -- free it, we'll drop it and make a bigger one
00156     freeRegion(region);
00157     region=new RCRegion(FILE_HEADER_SIZE+payload+sizeof(char)*filename.size()*2); //triple the allocation for filename so we don't have to resize if we get a longer name later
00158   }
00159   char* buf=region->Base();
00160   unsigned int remain=FILE_HEADER_SIZE;
00161   if(!LoadSave::encodeInc((unsigned int)verbose,buf,remain)) return NULL;
00162   if(!LoadSave::encodeInc(frameSN++,buf,remain)) return NULL;
00163   if(!LoadSave::encodeInc(filename,buf,remain)) return NULL;
00164   if(!LoadSave::encodeInc(hasMoreData,buf,remain)) return NULL;
00165   if(!LoadSave::encodeInc(payload,buf,remain)) return NULL;
00166   ASSERT(remain==0,"LoadDataThread::setupRegion(): Leftover bytes in header? FILE_HEADER_SIZE is wrong\n");
00167   return buf;
00168 }
00169 
00170 char* LoadDataThread::deserializeHeader(char* buf, unsigned int size, unsigned int* verbose, unsigned int* sn, std::string* filename, bool* dataInQueue, unsigned int* payloadSize) {
00171   if(verbose!=NULL) { if(!LoadSave::decodeInc(*verbose,buf,size)) return NULL; } else buf+=LoadSave::getSerializedSize(*verbose);
00172   if(sn!=NULL) { if(!LoadSave::decodeInc(*sn,buf,size)) return NULL; } else buf+=LoadSave::getSerializedSize(*sn);
00173   if(filename!=NULL) { if(!LoadSave::decodeInc(*filename,buf,size)) return NULL; } else {
00174     unsigned int len=0;
00175     if(!LoadSave::decodeInc(len,buf,size)) return NULL;
00176     buf+=len+1;
00177   }
00178   if(dataInQueue!=NULL) { if(!LoadSave::decodeInc(*dataInQueue,buf,size)) return NULL; } else buf+=LoadSave::getSerializedSize(*dataInQueue);
00179 #ifndef DEBUG
00180   if(payloadSize!=NULL) { if(!LoadSave::decodeInc(*payloadSize,buf,size)) return NULL; } else buf+=LoadSave::getSerializedSize(*payloadSize);
00181 #else
00182   unsigned int lPayloadSize; // want to error check payloadSize regardless of whether caller wants the info
00183   if(payloadSize==NULL)
00184     payloadSize=&lPayloadSize;
00185   if(!LoadSave::decodeInc(*payloadSize,buf,size)) return NULL;
00186   ASSERT(size>=*payloadSize,"short payload (" << size << " vs expected " << *payloadSize << ")");
00187   // this is normal (regions are generated a little bigger than they need to be to allow recycling)
00188   //ASSERT(size<=*payloadSize,"unhandled bytes after payload (" << size << " vs expected " << *payloadSize << ")");
00189 #endif
00190   return buf;
00191 }
00192 
00193 void LoadDataThread::sendHeartbeat(unsigned int curt) {
00194   RCRegion *unused=firstUnusedRegion();
00195   char * buf=setupRegion(unused,"heartbeat",0,curDS!=NULL && curDS->nextTimestamp()!=-1U);
00196   if(unused==NULL || buf==NULL)
00197     return;
00198   if(verbose>=3)
00199     std::cout << "Sending heartbeat at " << curt << std::endl;
00200   msgr.sendMessage(unused);
00201   regions.push_back(unused);
00202   lastSent=curt;
00203 }
00204 
00205 unsigned int LoadDataThread::calcNextHeartbeat(unsigned int curt) const {
00206   unsigned int skip=static_cast<unsigned int>((curt-lastSent)*framerate/1000);
00207   return static_cast<unsigned int>(lastSent+(skip+1)*1000.f/framerate);
00208 }
00209 
00210 std::string LoadDataThread::getAvailableDataSources() {
00211   std::string str;
00212   for(DeviceDriver::registry_t::const_iterator dit=DeviceDriver::getRegistry().begin(); dit!=DeviceDriver::getRegistry().end(); ++dit) {
00213     DeviceDriver * dd=DeviceDriver::getRegistry().getInstance(dit->first);
00214     if(dd!=NULL) {
00215       std::map<std::string,DataSource*> srcmap;
00216       (dd->*getDataSources)(srcmap);
00217       for(std::map<std::string,DataSource*>::const_iterator sit=srcmap.begin(); sit!=srcmap.end(); ++sit)
00218         str+=dit->first+'.'+sit->first+' ';
00219     }
00220   }
00221   std::string sourcehelp="Names a DeviceDriver instance from which data will be taken.\n"
00222     "Can be either just the driver name (use first data source), or 'DriverName.QueueName'.\n"
00223     "Available data sources: ";
00224   if(str.size()==0)
00225     sourcehelp+="(none available, see 'new' command to instantiate drivers)";
00226   else
00227     sourcehelp+=str;
00228   setComment("Source",sourcehelp);
00229   if(str.size()>0) // we actually want the extra space at the end in the plist::Dictionary comments, but we'll remove it before we return
00230     str.resize(str.size()-1);
00231   plistValueChanged(source); // check that our source is still valid (or if it wasn't valid, perhaps now it is)
00232   return str;
00233 }
00234 
00235 void LoadDataThread::setDataSource(DeviceDriver* dd, DataSource* ds) {
00236   if(curDS==ds && dsDriver==dd)
00237     return;
00238   if(dsDriver!=NULL)
00239     dsDriver->removeSourceListener(this);
00240   if(dd!=NULL)
00241     dd->addSourceListener(this);
00242   dsDriver=dd;
00243   if(curDS!=NULL) {
00244     curDS->setDataSourceThread(NULL);
00245     curDS=NULL;
00246     if(isStarted()) {
00247       stop();
00248       enabled=true; // retain enabled setting
00249       for(unsigned int checks=0; checks<8 && isStarted(); checks++)
00250         usleep(250*1000);
00251       if(isStarted()) {
00252         cerr << "Trying to close LoadDataThread's data source, looks like it's blocked, killing..." << endl;
00253         kill();
00254         join();
00255       }
00256     }
00257   }
00258   curDS=ds;
00259   sourceSN=0;
00260   lastSent=-1;
00261   if(curDS!=NULL) {
00262     curDS->setDataSourceThread(this);
00263     curDS->setDataSourceVerbose(verbose);
00264     curDS->setDataSourceFramerate(framerate);
00265     curDS->setFrozen(frozen);
00266     if(enabled)
00267       start();
00268   }
00269 }
00270 
00271 unsigned int LoadDataThread::runloop() {
00272   float nextT = (lastSent<0) ? get_time() : (lastSent+1000.f/framerate);
00273   const char* payload;
00274   unsigned int sz;
00275   unsigned int timestamp=static_cast<unsigned int>(nextT+.5f);
00276   std::string name;
00277   {
00278     MarkScope l(lock);
00279     unsigned int curt=get_time();
00280     if(frozen || curDS==NULL) {
00281       if(heartbeat && curt>=nextT)
00282         sendHeartbeat(curt);
00283       return calcSleepTime();
00284     }
00285     runloopState=GET_FRAME;
00286   }
00287   unsigned int sn=curDS->getData(payload, sz, timestamp, name);
00288   if(curDS==NULL) // broke out, our source has been marked invalid by another thread
00289     return -1U;
00290   unsigned int curt;
00291   if(payload!=NULL) {
00292     if(verbose>=1 && sn!=sourceSN+1 && sourceSN!=0)
00293       std::cout << "Data Source dropped " << (sn-sourceSN-1) << " frames"  << std::endl;
00294     sourceSN=sn;
00295     // if we have a payload and timestamp is still in the future, wait for it
00296     {
00297       MarkScope l(lock); // don't hold lock throughout sleep, just on updating runloopState
00298       runloopState=GOT_FRAME;
00299     }
00300     while(timestamp>(curt=get_time()) && runloopState!=ADVANCE_FRAME) {
00301       //cout << "size " << sz << " timestamp " << timestamp << " vs " << curt << " name " << name << endl;
00302       int res=usleep(static_cast<unsigned int>((timestamp-curt)*1000/getTimeScale()));
00303       if(res!=0) {
00304         if(errno==EINTR && runloopState==ADVANCE_FRAME) {
00305           break;
00306         } else {
00307           perror("LoadDataThread::runloop() nanosleep");
00308           break;
00309         }
00310       }
00311     }
00312   }
00313   {
00314     MarkScope l(lock);
00315     curt=get_time(); //recheck time, lock might've taken a while
00316     if(payload==NULL) {
00317       if(heartbeat && curt>=nextT)
00318         sendHeartbeat(curt);
00319       runloopState=INTERFRAME;
00320       return calcSleepTime();
00321     }
00322     if(verbose>=2)
00323       std::cout << "Sending frame: \"" << name << "\" (req. " << static_cast<unsigned int>(nextT+.5f) << ", sched. " << timestamp << ", now " << curt << ')' << std::endl;
00324     RCRegion * r=firstUnusedRegion();
00325     char * buf=setupRegion(r,name,sz,curDS->nextTimestamp()!=-1U);
00326     ASSERTRETVAL(buf!=NULL,"setupRegion returned null region",false);
00327     memcpy(buf,payload,sz);
00328     //if(sz < 1024)
00329     //  cout << "size: " << sz << " payload:" << payload << endl;
00330     msgr.sendMessage(r);
00331     regions.push_back(r);
00332     if(curt - nextT > 1000.f/framerate*5)
00333       lastSent=curt; // apparently can't keep up, don't let requested time fall ridiculously behind...
00334     else
00335       lastSent=nextT;
00336     runloopState=INTERFRAME;
00337     return 0;
00338   }
00339 }
00340 
00341 
00342 /*! @file
00343  * @brief 
00344  * @author Ethan Tira-Thompson (ejt) (Creator)
00345  *
00346  * $Author: ejt $
00347  * $Name: tekkotsu-4_0 $
00348  * $Revision: 1.18 $
00349  * $State: Exp $
00350  * $Date: 2007/10/12 16:55:04 $
00351  */

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4