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
00012 using namespace std;
00013
00014 WorldStatePool::WorldStatePool() : order(), lock() {
00015
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
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
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
00096
00097 static UpdateInfo info;
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;
00106
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
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 , bool ) {
00120
00121
00122 static UpdateInfo info;
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
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;
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
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
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) {
00155
00156
00157 ASSERT(feedbackGenerated || feedback==NULL, "feedbackGenerated is false, yet feedback was supplied?");
00158 if(wsw.getComplete()) {
00159
00160
00161 return true;
00162 }
00163 ASSERT((wsw.getStatus()&FEEDBACK_APPLIED)==0, "feedback applied, but apparently not completed "<<wsw.getComplete());
00164
00165
00166 if((wsw.getStatus()&SENSORS_APPLIED) != 0) {
00167
00168 if(feedback!=NULL) {
00169
00170 if(zeroPIDFeedback) {
00171
00172 for(unsigned int i=0; i<NumOutputs; i++)
00173 state->outputs[i]=feedback->getOutputCmd(i).value;
00174 } else {
00175
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
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
00189 static PostureEngine pose;
00190 if(info.payload==NULL) {
00191
00192
00193 pose.setLoadedSensors(wsw.src);
00194 if(!feedbackGenerated) {
00195
00196 pose.takeSnapshot(*wsw.src);
00197 pose.setWeights(1);
00198 } else if(zeroPIDFeedback) {
00199
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 }
00205 } else {
00206
00207 if(feedback!=NULL) {
00208
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
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;
00226
00227 pose.setLoadedSensors(state);
00228
00229
00230 size_t x=pose.loadBuffer(info.payload,info.payloadSize);
00231
00232 if(!x) {
00233 cerr << "ERROR: Corrupted sensor readings received by Main" << endl;
00234 return false;
00235 }
00236 state->frameNumber=stateFrame;
00237
00238 if(stateFrame>wsw.src->frameNumber)
00239 return false;
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
00244
00245
00246
00247 } else if(zeroPIDFeedback) {
00248
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 }
00254 } else {
00255
00256 if(feedback!=NULL) {
00257
00258 for(unsigned int i=0; i<NumPIDJoints; ++i)
00259 if(!isZeroPID(wsw.src,i))
00260 pose(i+PIDJointOffset)=feedback->getOutputCmd(i).value;
00261
00262
00263 wsw.setStatus(wsw.getStatus() | FEEDBACK_APPLIED);
00264 } else {
00265
00266 for(unsigned int i=0; i<NumPIDJoints; ++i)
00267 if(!isZeroPID(wsw.src,i))
00268 pose(i+PIDJointOffset).weight=0;
00269
00270
00271 }
00272 }
00273 }
00274 state->read(pose,wsw.src,erouter);
00275
00276 state->frameNumber=(wsw.frame+1)*NumFrames+1;
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
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) {
00294
00295 reading[toUse]--;
00296 }
00297 toUse=order.back();
00298 reading[toUse]++;
00299 }
00300 #ifdef PLATFORM_APERIOS
00301 MarkScope wl(complete[toUse]);
00302 #else
00303 MessageQueueBase::getSemaphoreManager()->testZero(complete[toUse],true);
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
00317 std::cerr << " falling into most recent buffer" << std::endl;
00318 toUse=order.prev(order.end());
00319 }
00320 reading[toUse]++;
00321 }
00322
00323
00324 #ifdef DEBUG
00325 if(toUse==-1U) {
00326 std::cerr << "ERROR: WorldStatePool: toUse was not chosen!" << std::endl;
00327
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
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
00352 idx=order.prev(order.end());
00353 unsigned int toUse=order[idx];
00354 if(frames[toUse]>frame) {
00355
00356
00357
00358 return -1U;
00359 }
00360 if(frames[toUse]!=frame || !block) {
00361
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) {
00370 if(reading[toUse]==0)
00371 break;
00372 } else if(fallback==order.end())
00373 fallback=idx;
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 {
00390 MarkScope l(lock);
00391 order_t::index_t idx;
00392 toUse=selectWriteState(frame,block,idx);
00393 if(toUse==-1U)
00394 return -1U;
00395
00396
00397 writing[toUse]++;
00398 if(frames[toUse]!=frame) {
00399
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();
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()) {
00419
00420
00421
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
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
00446
00447
00448
00449
00450
00451
00452
00453
00454