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);
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;
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);
00087 }
00088 }
00089
00090 const std::string& FileSystemDataSource::getUsedPath() const { return (path.size()==0) ? parent.path : path; }
00091
00092 void FileSystemDataSource::loadFileList(bool clearCurrent, bool reportMissing) {
00093 nextTime=freezeTime=get_time();
00094 struct stat sb;
00095 if(clearCurrent)
00096 clearFiles();
00097 else if(files.size()==0)
00098 clearCurrent=true;
00099 if(getUsedPath().size()==0)
00100 return;
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
00110 try {
00111 if(string_util::reMatch(getUsedPath(),filenameFilter))
00112 loadSingleFile(getUsedPath().c_str());
00113 else {
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) {
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) {
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
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 }
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
00288 float tinc=1000.f/framerate;
00289 for(std::set<std::string>::const_iterator it=dirfiles.begin(); it!=dirfiles.end(); ++it) {
00290
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)
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
00334 } else if(files.size()>0) {
00335 files.back()->lifetime+=curtime;
00336
00337 } else {
00338 initialDelay=curtime;
00339 nextTime=get_time()+curtime;
00340
00341 }
00342 }
00343 if(fn[0]!='/') {
00344 string::size_type srcdir=getUsedPath().rfind('/');
00345 if(srcdir!=string::npos)
00346 fn=getUsedPath().substr(0,srcdir+1)+fn;
00347 }
00348
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 }
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;
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
00466
00467
00468
00469
00470
00471
00472
00473
00474