Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

FileSystemDataSource.cc

Go to the documentation of this file.
00001 #include "FileSystemDataSource.h"
00002 #include "local/DeviceDrivers/LoggedDataDriver.h"
00003 #include "Shared/get_time.h"
00004 #include "Shared/RobotInfo.h"
00005 #include "Shared/string_util.h"
00006 #include <set>
00007 #include <fstream>
00008 #include <sys/types.h>
00009 #include <sys/mman.h>
00010 #include <sys/stat.h>
00011 #include <regex.h>
00012 #include <dirent.h>
00013 #include <fcntl.h>
00014 #include <sys/resource.h>
00015 #include <cmath>
00016 #include <errno.h>
00017 
00018 using namespace std; 
00019 
00020 FileSystemDataSource::~FileSystemDataSource() {
00021   clearFiles();
00022 }
00023 
00024 unsigned int FileSystemDataSource::nextTimestamp() {
00025   return (curfile!=files.end()) ? static_cast<unsigned int>(nextTime) : -1U;
00026 }
00027 
00028 const std::string& FileSystemDataSource::nextName() {
00029   if(curfile!=files.end())
00030     return (*curfile)->filename;
00031   else {
00032     static const std::string noneStr="(none)";
00033     return noneStr;
00034   }
00035 }
00036 
00037 unsigned int FileSystemDataSource::getData(const char *& payload, unsigned int& payloadSize, unsigned int& timestamp, std::string& name) {
00038   if(curfile==files.end()) {
00039     payload=NULL; payloadSize=0;
00040     return sn;
00041   }
00042   if(nextTime<timestamp && std::abs(static_cast<int>(timestamp-nextTime))>std::abs(timestamp-(nextTime+(*curfile)->lifetime))) {
00043     float looptime=getLoopTime(true);
00044     if(looptime>0) {
00045       while(nextTime+looptime<timestamp)
00046         nextTime+=looptime;
00047     }
00048     while(nextTime<timestamp && std::abs(static_cast<int>(timestamp-nextTime))>std::abs(timestamp-(nextTime+(*curfile)->lifetime))) {
00049       nextFrame(0); // too late, drop frames
00050       sn++;
00051     }
00052   }
00053   if(!(*curfile)->prepared)
00054     (*curfile)->prepare();
00055   payload=(*curfile)->data;
00056   payloadSize=(*curfile)->size;
00057   unsigned int reqTime=timestamp;
00058   float origNext=nextTime;
00059   timestamp=static_cast<unsigned int>(nextTime);
00060   name=(*curfile)->filename;
00061   nextFrame();
00062   if(reqTime==0 && timestamp!=0 && (frozen || getTimeScale()>=0))
00063     nextTime=origNext; // the requested time is 0, this is an advance frame, hold nextTime unless not frozen and full speed mode
00064   return sn++;
00065 }
00066 
00067 void FileSystemDataSource::setDataSourceThread(LoadDataThread* th) {
00068   bool toggle=(th!=NULL && thread==NULL || th==NULL && thread!=NULL);
00069   DataSource::setDataSourceThread(th);
00070   nextTime=get_time();
00071   if(toggle)
00072     updateProvidingOutputs();
00073 }
00074 
00075 void FileSystemDataSource::setDataSourceFramerate(float fr) {
00076   DataSource::setDataSourceFramerate(fr);
00077   if(!usingIndexFile()) {
00078     const float dt=1000.f/framerate;
00079     bool first = (curfile==files.begin());
00080     if(!first)
00081       nextTime-=(*--curfile)->lifetime;
00082     for(files_t::const_iterator it=files.begin(); it!=files.end(); ++it)
00083       (*it)->lifetime=dt;
00084     if(!first)
00085       nextTime+=(*curfile++)->lifetime;
00086     parent.plistValueChanged(path); // to reset the loop time if sharing a source
00087   }
00088 }
00089 
00090 const std::string& FileSystemDataSource::getUsedPath() const { return (path.size()==0) ? parent.path : path; }
00091 
00092 void FileSystemDataSource::loadFileList(bool clearCurrent/*=true*/, bool reportMissing/*=true*/) {
00093   nextTime=freezeTime=get_time();
00094   struct stat sb;
00095   if(clearCurrent)
00096     clearFiles();
00097   else if(files.size()==0)
00098     clearCurrent=true; // was empty, pretend we just cleared it, have to do the same re-initialization
00099   if(getUsedPath().size()==0)
00100     return; //empty path means disabled
00101   if(stat(getUsedPath().c_str(),&sb)) {
00102     if(reportMissing)
00103       std::cerr << "FileSystemDataSource could not access path '" << getUsedPath() << "'" << std::endl;
00104     return;
00105   }
00106   if(sb.st_mode&S_IFDIR) {
00107     loadFileListFromDirectory();
00108   } else {
00109     //Test to see if the file matches the filter
00110     try {
00111       if(string_util::reMatch(getUsedPath(),filenameFilter))
00112         loadSingleFile(getUsedPath().c_str());
00113       else { //if it doesn't match the image RE, assume it's an index file
00114         if(!loadFileListFromIndex())
00115           std::cerr << "Source '" << getUsedPath() << "' does not match the filename filter '" << filenameFilter << "' and is not an index list." << std::endl;
00116       }
00117     } catch(const std::string& err) {
00118       std::cerr << err << std::endl;
00119     }
00120   }
00121   if(clearCurrent) {
00122     curfile=files.begin();
00123     files_t::iterator it=curfile;
00124     for(unsigned int numPreload=2; numPreload>0 && it!=files.end(); numPreload--) {
00125       (*it)->prepare();
00126       if(++it==files.end() && loop)
00127         it=files.begin();
00128     }
00129   }
00130   actualLoopTime=naturalLoopTime=calcLoopTime();
00131 }
00132 
00133 void FileSystemDataSource::setFrame(unsigned int f, unsigned int numPreload/*=2*/) {
00134   for(;curfile!=files.end() && (*curfile)->prepared; ++curfile) {
00135     if(files.size()>MAX_LOAD)
00136       (*curfile)->release();
00137     else
00138       (*curfile)->done();
00139   }
00140   nextTime=freezeTime=get_time();
00141   curfile=files.begin();
00142   advance(curfile,f);
00143   files_t::iterator it=curfile;
00144   for(; numPreload>0 && it!=files.end(); numPreload--) {
00145     (*it)->prepare();
00146     if(++it==files.end() && loop)
00147       it=files.begin();
00148   }
00149 }
00150 
00151 void FileSystemDataSource::nextFrame(unsigned int numPreload/*=2*/) {
00152   if(numPreload==0 && verbose>=2)
00153     cout << "Dropping " << (*curfile)->filename << ' ' << nextTime << ' ' << (*curfile)->lifetime << endl;
00154   if(files.size()>MAX_LOAD)
00155     (*curfile)->release();
00156   else
00157     (*curfile)->done();
00158   nextTime+=(*curfile)->lifetime;
00159   if(++curfile==files.end() && loop) {
00160     nextTime+=initialDelay;
00161     curfile=files.begin();
00162     if(verbose>=3)
00163       cout << "Looping file system data source at " << nextTime << " to " << (*curfile)->filename << " (loop time=" << getLoopTime() << ")" << endl;
00164   }
00165   files_t::iterator it=curfile;
00166   for(; numPreload>0 && it!=files.end(); numPreload--) {
00167     (*it)->prepare();
00168     if(++it==files.end() && loop)
00169       it=files.begin();
00170   }
00171 }
00172 
00173 float FileSystemDataSource::calcLoopTime() const {
00174   if(files.size()==0)
00175     return 0;
00176   float t=initialDelay;
00177   for(files_t::const_iterator it=files.begin(); it!=files.end(); ++it)
00178     t+=(*it)->lifetime;
00179   return t;
00180 }
00181 void FileSystemDataSource::setLoopTime(float t) {
00182   if(files.size()==0)
00183     return;
00184   float remain = t - getLoopTime(true);
00185   if(remain + files.back()->lifetime < 0) {
00186     std::cerr << "FileSystemDataSource::setLoopTime(" << t << ") would result in a negative frame lifetime" << std::endl;
00187     return;
00188   }
00189   files.back()->lifetime+=remain;
00190   actualLoopTime=t;
00191 }
00192 
00193 void FileSystemDataSource::clearFiles() {
00194   // FileInfo destructor should take care of deleting data buffers...
00195   for(files_t::iterator it=files.begin(); it!=files.end(); ++it)
00196     delete *it;
00197   files.clear();
00198   curfile=files.begin();
00199   initialDelay=0;
00200   actualLoopTime=naturalLoopTime=0;
00201 }
00202 
00203 void FileSystemDataSource::plistValueChanged(const plist::PrimitiveBase& pl) {
00204   if(&pl==&path) {
00205     loadFileList();
00206   } else {
00207     cerr << "FileSystemDataSource didn't handle call to plistValueChanged for " << pl.get() << endl;
00208   }
00209 }
00210 
00211 void FileSystemDataSource::loadXML(xmlNode* node) {
00212   path.removePrimitiveListener(this);
00213   plist::Dictionary::loadXML(node);
00214   path.addPrimitiveListener(this);
00215   plistValueChanged(path);
00216 }
00217 
00218 void FileSystemDataSource::updateProvidingOutputs() {
00219   if(thread!=NULL) {
00220     for(unsigned int i=0; i<NumOutputs; i++)
00221       providingOutput(i);
00222   } else {
00223     for(unsigned int i=0; i<NumOutputs; i++)
00224       ignoringOutput(i);
00225   }
00226 }
00227 
00228 void FileSystemDataSource::loadSingleFile(const std::string& file) {
00229   indexed=false;
00230   enqueueFile(file,1000.f/framerate);
00231 }
00232 
00233 void FileSystemDataSource::loadFileListFromDirectory() {
00234   indexed=false;
00235   regex_t re;
00236   if(int err=regcomp(&re,filenameFilter.c_str(),REG_EXTENDED | REG_NOSUB)) {
00237     char msg[128];
00238     regerror(err,&re,msg,128);
00239     std::cerr << "Bad filter '" << filenameFilter << "': " << msg << std::endl;
00240     regfree(&re);
00241     return;
00242   }
00243   DIR * d=opendir(getUsedPath().c_str());
00244   if(d==NULL) {
00245     std::cerr << "Could not open directory " << getUsedPath() << std::endl;
00246     regfree(&re);
00247     return;
00248   }
00249   struct dirent* res;
00250   
00251 #ifdef HAVE_READDIR_R
00252   struct dirent cur;
00253   if(readdir_r(d,&cur,&res)) {
00254     std::cerr << "Error reading files from " << getUsedPath() << std::endl;
00255     closedir(d);
00256     regfree(&re);
00257     return;
00258   }
00259 #else
00260   res=readdir(d);
00261 #endif
00262 
00263   std::set<std::string> dirfiles;
00264   while(res!=NULL) {
00265     int match=regexec(&re,res->d_name,0,NULL,0);
00266     if(match==0) {
00267       dirfiles.insert(res->d_name);
00268     } else if(match!=REG_NOMATCH) {
00269       char msg[128];
00270       regerror(match,&re,msg,128);
00271       std::cerr << "Regex error on '" << res->d_name << "': " << msg << std::endl;
00272     } // else std::cout << "Skipping " << res->d_name << std::endl;
00273 #ifdef HAVE_READDIR_R
00274     if(readdir_r(d,&cur,&res)) {
00275       std::cerr << "Error reading files from " << getUsedPath() << std::endl;
00276       closedir(d);
00277       regfree(&re);
00278       return;
00279     }
00280 #else
00281     res=readdir(d);
00282 #endif
00283   }
00284   closedir(d);
00285   regfree(&re);
00286   
00287   //std::cout << "Processing " << getUsedPath() << std::endl;
00288   float tinc=1000.f/framerate;
00289   for(std::set<std::string>::const_iterator it=dirfiles.begin(); it!=dirfiles.end(); ++it) {
00290     //std::cout << "Enqueuing " << *it << std::endl;
00291     enqueueFile((getUsedPath()+"/")+(*it),tinc);
00292   }
00293 }
00294 
00295 bool FileSystemDataSource::loadFileListFromIndex() {
00296   indexed=(indexed || files.size()==0);
00297   regex_t re;
00298   if(int err=regcomp(&re,filenameFilter.c_str(),REG_EXTENDED | REG_NOSUB)) {
00299     char msg[128];
00300     regerror(err,&re,msg,128);
00301     std::cerr << "Bad filter '" << filenameFilter << "': " << msg << std::endl;
00302     regfree(&re);
00303     return false;
00304   }
00305   
00306   ifstream in(getUsedPath().c_str());
00307   string cur;
00308   getline(in,cur);
00309   if(cur.find("First frame ")==0) //skip the header line from the GUI, e.g. 'First frame 42898 timestamp: 1439018'
00310     getline(in,cur);
00311   
00312   float tinc=1000.f/framerate;
00313   float lasttime=-tinc;
00314   while(in) {
00315     string fn = cur.substr(0,cur.find('\t'));
00316     int match=regexec(&re,fn.c_str(),0,NULL,0);
00317     if(match==0) {
00318       float curtime=lasttime+tinc;
00319       if(fn.size()!=cur.size()) {
00320         const char * timep=cur.c_str()+cur.rfind('\t');
00321         char * endp=NULL;
00322         curtime=strtof(timep,&endp);
00323         if(timep==endp) {
00324           std::cerr << "ERROR: '" << getUsedPath() << "' does not seem to be a valid index file." << std::endl;
00325           std::cerr << "       Use output from VisionGUI, or use format 'filename <tab> time'" << std::endl;
00326           std::cerr << "       Where 'time' is the time in milliseconds at which the file should be processed, relative" << std::endl;
00327           std::cerr << "       to the time at which the index file is loaded." << std::endl;
00328           regfree(&re);
00329           return false;
00330         }
00331         if(lasttime>=0) {
00332           files.back()->lifetime=curtime-lasttime;
00333           //std::cout << "(previous frame lifetime " << files.back()->lifetime << ") ";
00334         } else if(files.size()>0) {
00335           files.back()->lifetime+=curtime;
00336           //std::cout << "(previous frame increased lifetime to " << files.back()->lifetime << ") ";
00337         } else {
00338           initialDelay=curtime;
00339           nextTime=get_time()+curtime;
00340           //std::cout << "nextTime set to " << nextTime << " ";
00341         }
00342       }
00343       if(fn[0]!='/') { // if not absolute path, tack on path to index file (*do this after previous check*!)
00344         string::size_type srcdir=getUsedPath().rfind('/');
00345         if(srcdir!=string::npos)
00346           fn=getUsedPath().substr(0,srcdir+1)+fn;
00347       }
00348       //std::cout << "Enqueuing " << fn << " at " << curtime << endl;
00349       enqueueFile(fn,tinc);
00350       lasttime=curtime;
00351     } else if(match!=REG_NOMATCH) {
00352       char msg[128];
00353       regerror(match,&re,msg,128);
00354       std::cerr << "Regex error on '" << fn << "': " << msg << std::endl;
00355     } // else std::cout << "Skipping " << res->d_name << std::endl;
00356     getline(in,cur);
00357   }
00358   regfree(&re);
00359   return true;
00360 }
00361 
00362 char * FileSystemDataSource::loadData(const std::string& file) {
00363   struct stat statbuf;
00364   if(stat(file.c_str(),&statbuf)!=0) {
00365     perror("LoadDataThread::loadData");
00366     return NULL;
00367   }
00368   FILE * f=fopen(file.c_str(),"rb");
00369   if(f==NULL) {
00370     std::cerr << "LoadDataThread::loadData(): File open failed" << std::endl;
00371     return NULL;
00372   }
00373   char* buf=new char[statbuf.st_size];
00374   int nread=fread(buf,1,statbuf.st_size,f);;
00375   fclose(f);
00376   f=NULL;
00377   if(nread!=statbuf.st_size) {
00378     std::cerr << "LoadDataThread::,datagram(): failed to load entire file, "<<nread<<" read, "<<statbuf.st_size<<" requested" << std::endl;
00379     return NULL;
00380   }
00381   return buf;
00382 }
00383 
00384 void FileSystemDataSource::FileInfo::prepare() {
00385   if(prepared)
00386     return;
00387   if(data==NULL) {
00388     struct stat statbuf;
00389     if(stat(filename.c_str(),&statbuf)!=0) {
00390       std::string err="FileSystemDataSource::FileInfo::prepare() failed to stat file ";
00391       err+=filename;
00392       perror(err.c_str());
00393       return;
00394     }
00395     int fd=open(filename.c_str(),O_RDONLY);
00396     if(fd<0) {
00397       std::string err="FileSystemDataSource::FileInfo::prepare() unable to open file ";
00398       err+=filename;
00399       perror(err.c_str());
00400       return;
00401     }
00402     size=statbuf.st_size;
00403     data=static_cast<char*>(mmap(NULL,size,PROT_READ,MAP_PRIVATE|MAP_FILE,fd,0));
00404     if(data==MAP_FAILED) {
00405       data=NULL;
00406       size=0;
00407       std::string err="FileSystemDataSource::FileInfo::prepare() unable to mmap file ";
00408       err+=filename;
00409       perror(err.c_str());
00410       return;
00411     }
00412     if(close(fd)!=0) {
00413       std::string err="FileSystemDataSource::FileInfo::prepare() unable to close file ";
00414       err+=filename;
00415       perror(err.c_str());
00416       return;
00417     }
00418   }
00419   if(mlock(data,size)!=0) {
00420     if(errno==ENOMEM) {
00421       static bool firsterr=true; // give a warning just the first time if mlock fails because RLIMIT_MEMLOCK is too low
00422       if(firsterr) {
00423         firsterr=false;
00424         rlimit rl;
00425 #ifndef __CYGWIN__
00426         getrlimit(RLIMIT_MEMLOCK,&rl);
00427         cerr << "Notice: mlock() failed because RLIMIT_MEMLOCK is too low, limited to " << (rl.rlim_cur/1024) << "KB\n"
00428              << "Increasing this limit can smooth logged data I/O in low memory situations. (see ulimit/limit commands)" << endl;
00429 #endif
00430       }
00431     } else {
00432       std::string err="FileSystemDataSource::FileInfo::prepare() unable to mlock file ";
00433       err+=filename;
00434       perror(err.c_str());
00435     }
00436     return;
00437   }
00438   prepared=true;
00439 }
00440 
00441 void FileSystemDataSource::FileInfo::done() {
00442   if(data==NULL || !prepared)
00443     return;
00444   prepared=false;
00445   if(munlock(data,size)!=0) {
00446     std::string err="FileSystemDataSource::FileInfo::prepare() unable to munlock file ";
00447     err+=filename;
00448     perror(err.c_str());
00449   }
00450 }
00451 
00452 void FileSystemDataSource::FileInfo::release() {
00453   if(data==NULL)
00454     return;
00455   done();
00456   if(munmap(data,size)!=0) {
00457     std::string err="FileSystemDataSource::FileInfo::prepare() unable to munmap file ";
00458     err+=filename;
00459     perror(err.c_str());
00460   }
00461   data=NULL;
00462   size=0;
00463 }
00464 
00465 /*! @file
00466  * @brief 
00467  * @author Ethan Tira-Thompson (ejt) (Creator)
00468  *
00469  * $Author: ejt $
00470  * $Name: tekkotsu-4_0 $
00471  * $Revision: 1.4 $
00472  * $State: Exp $
00473  * $Date: 2007/11/13 04:16:05 $
00474  */

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