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
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
00037 if(runloopState==GET_FRAME) {
00038 runloopState=ADVANCE_FRAME;
00039 return true;
00040 } else if(runloopState==GOT_FRAME) {
00041 runloopState=ADVANCE_FRAME;
00042 interrupt();
00043 return true;
00044 }
00045
00046
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;
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;
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
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);
00154 else if(region->Size()<FILE_HEADER_SIZE+payload) {
00155
00156 freeRegion(region);
00157 region=new RCRegion(FILE_HEADER_SIZE+payload+sizeof(char)*filename.size()*2);
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;
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
00188
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)
00230 str.resize(str.size()-1);
00231 plistValueChanged(source);
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;
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)
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
00296 {
00297 MarkScope l(lock);
00298 runloopState=GOT_FRAME;
00299 }
00300 while(timestamp>(curt=get_time()) && runloopState!=ADVANCE_FRAME) {
00301
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();
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
00329
00330 msgr.sendMessage(r);
00331 regions.push_back(r);
00332 if(curt - nextT > 1000.f/framerate*5)
00333 lastSent=curt;
00334 else
00335 lastSent=nextT;
00336 runloopState=INTERFRAME;
00337 return 0;
00338 }
00339 }
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351