00001 #include "Main.h"
00002 #include "SoundPlay.h"
00003 #include "Motion.h"
00004 #include "Simulator.h"
00005 #include "TimerExecThread.h"
00006 #include "local/LoadDataThread.h"
00007 #include "SimConfig.h"
00008 #include "MotionExecThread.h"
00009
00010 #include "IPC/RegionRegistry.h"
00011 #include "IPC/MessageReceiver.h"
00012 #include "IPC/PollThread.h"
00013 #include "Motion/Kinematics.h"
00014 #include "Motion/PostureEngine.h"
00015 #include "Wireless/Wireless.h"
00016 #include "Shared/ProjectInterface.h"
00017 #include "Behaviors/BehaviorBase.h"
00018 #include "Events/DataEvent.h"
00019 #include "Events/EventRouter.h"
00020 #include "Shared/Config.h"
00021 #include "Shared/MarkScope.h"
00022
00023 #include "Events/EventBase.h"
00024 #include "Events/LocomotionEvent.h"
00025 #include "Events/TextMsgEvent.h"
00026 #include "Events/VisionObjectEvent.h"
00027
00028 using namespace std;
00029
00030 Main::Main()
00031 : Process(getID(),getClassName()),
00032 sounds(ipc_setup->registerRegion(SoundPlay::getSoundPlayID(),sizeof(sim::SoundPlayQueue_t))),
00033 motions(ipc_setup->registerRegion(Motion::getMotionCommandID(),sizeof(sim::MotionCommandQueue_t))),
00034 events(ipc_setup->registerRegion(getEventsID(),sizeof(sim::EventQueue_t))),
00035 cameraFrames(ipc_setup->registerRegion(Simulator::getCameraQueueID(),sizeof(sim::CameraQueue_t))),
00036 sensorFrames(ipc_setup->registerRegion(Simulator::getSensorQueueID(),sizeof(sim::SensorQueue_t))),
00037 timerWakeup(ipc_setup->registerRegion(Simulator::getTimerWakeupID(),sizeof(sim::TimerWakeup_t))),
00038 statusRequest(ipc_setup->registerRegion(Simulator::getStatusRequestID(),sizeof(sim::StatusRequest_t))),
00039 motionmanager(ipc_setup->registerRegion(Motion::getMotionManagerID(),sizeof(MotionManager))),
00040 soundmanager(ipc_setup->registerRegion(SoundPlay::getSoundManagerID(),sizeof(SoundManager))),
00041 worldstatepool(ipc_setup->registerRegion(getWorldStatePoolID(),sizeof(WorldStatePool))),
00042 motionProf(ipc_setup->registerRegion(Motion::getMotionProfilerID(),sizeof(motionProfiler_t))),
00043 soundProf(ipc_setup->registerRegion(SoundPlay::getSoundProfilerID(),sizeof(soundProfiler_t))),
00044 visrecv(NULL), sensrecv(NULL), evtrecv(NULL), timerrecv(NULL), statusrecv(NULL), timerExec(NULL),
00045 visionRead(true), wireless_thread(), worldStateCache(), behaviorLock(*worldstatepool),
00046 curimgregion(NULL), img(), lastVisionSN(-1U), lastSensorSN(-1U), lastSensorUpdateTime(0)
00047 {
00048 new (&(*events)) sim::EventQueue_t;
00049 new (&(*worldstatepool)) WorldStatePool;
00050 state=NULL;
00051 motman=&(*motionmanager);
00052 sndman=&(*soundmanager);
00053 ::mainProfiler=new mainProfiler_t;
00054 ::motionProfiler=&(*motionProf);
00055 ::soundProfiler=&(*soundProf);
00056
00057 if(sim::config.multiprocess) {
00058
00059 ASSERT(wireless==NULL,"global wireless already initialized before Main?");
00060 wireless = new Wireless();
00061 sout=wireless->socket(Socket::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*12);
00062 serr=wireless->socket(Socket::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*4);
00063 wireless->setDaemon(sout);
00064 wireless->setDaemon(serr);
00065 serr->setFlushType(Socket::FLUSH_BLOCKING);
00066 sout->setTextForward();
00067 serr->setForward(sout);
00068
00069
00070 ASSERT(kine==NULL,"global kine already initialized before Main?");
00071 kine=new Kinematics();
00072 }
00073 wireless->setCallbackLock(behaviorLock);
00074
00075
00076 }
00077
00078 Main::~Main() {
00079 if(sim::config.multiprocess) {
00080 delete wireless;
00081 wireless=NULL;
00082 delete kine;
00083 kine=NULL;
00084 }
00085 }
00086
00087
00088 void Main::DoStart() {
00089 try {
00090 Process::DoStart();
00091
00092
00093 sndman->InitAccess(*sounds);
00094 motman->InitAccess(*motions,behaviorLock);
00095
00096 wireless->listen(sout, config->main.console_port);
00097 wireless->listen(serr, config->main.stderr_port);
00098 wireless_thread.start();
00099 statusrecv=new MessageReceiver(*statusRequest,statusReport);
00100
00101 if(globals->waitForSensors)
00102 erouter->addListener(this,EventBase::sensorEGID);
00103 evtrecv=new MessageReceiver(*events,gotEvent,false);
00104 visrecv=new MessageReceiver(*cameraFrames,gotCamera,false);
00105 sensrecv=new MessageReceiver(*sensorFrames,gotSensors,false);
00106 timerrecv=new MessageReceiver(*timerWakeup,gotTimer,false);
00107 timerExec=new TimerExecThread(behaviorLock,false);
00108
00109 } catch(const std::exception& ex) {
00110 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during Main DoStart",&ex))
00111 throw;
00112 } catch(...) {
00113 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during Main DoStart",NULL))
00114 throw;
00115 }
00116 }
00117
00118 void Main::run() {
00119 try {
00120 evtrecv->start();
00121 if(globals->waitForSensors) {
00122 sensrecv->start();
00123 globals->waitSensors();
00124 }
00125
00126
00127
00128 if(globals->isShutdown())
00129 return;
00130
00131 {
00132 MarkScope bl(behaviorLock);
00133 ProjectInterface::startupBehavior().DoStart();
00134 globals->setNextTimer(erouter->getNextTimer());
00135 }
00136
00137 if(!globals->waitForSensors)
00138 sensrecv->start();
00139 visrecv->start();
00140 timerrecv->start();
00141 timerExec->reset();
00142
00143
00144 cout << sim::config.cmdPrompt << flush;
00145
00146 Process::run();
00147
00148 sensrecv->finish();
00149 visrecv->finish();
00150 evtrecv->finish();
00151 timerrecv->finish();
00152
00153 {
00154 MarkScope bl(behaviorLock);
00155 ProjectInterface::startupBehavior().DoStop();
00156 globals->setNextTimer(erouter->getNextTimer());
00157 }
00158 timerExec->reset();
00159
00160 } catch(const std::exception& ex) {
00161 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during Main 'run' runlevel (startupBehavior initialization and startup)",&ex))
00162 throw;
00163 } catch(...) {
00164 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during Main 'run' runlevel (startupBehavior initialization and startup)",NULL))
00165 throw;
00166 }
00167 }
00168
00169 void Main::DoStop() {
00170 try {
00171
00172 delete sensrecv;
00173 sensrecv=NULL;
00174 delete visrecv;
00175 visrecv=NULL;
00176 delete evtrecv;
00177 evtrecv=NULL;
00178 delete timerrecv;
00179 timerrecv=NULL;
00180 delete statusrecv;
00181 statusrecv=NULL;
00182
00183 motman->RemoveAccess();
00184
00185 if(curimgregion!=NULL)
00186 curimgregion->RemoveReference();
00187
00188 wireless_thread.stop();
00189 wireless_thread.join();
00190 wireless->setDaemon(sout,false);
00191 wireless->close(sout);
00192 sout=NULL;
00193 wireless->setDaemon(serr,false);
00194 wireless->close(serr);
00195 serr=NULL;
00196
00197 Process::DoStop();
00198
00199 } catch(const std::exception& ex) {
00200 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during Main DoStop",&ex))
00201 throw;
00202 } catch(...) {
00203 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during Main DoStop",NULL))
00204 throw;
00205 }
00206 }
00207
00208 void Main::processEvent(const EventBase&) {
00209 erouter->removeListener(this);
00210 for(unsigned int i=0; i<NumOutputs; i++)
00211 motman->setOutput(NULL,i,state->outputs[i]);
00212 globals->signalHaveSensors();
00213 }
00214
00215 bool Main::gotCamera(RCRegion* msg) {
00216 if(msg==NULL)
00217 return true;
00218 Main * main=dynamic_cast<Main*>(Process::getCurrent());
00219 ASSERTRETVAL(main!=NULL,"gotCamera, but not within Main process!",true);
00220 MarkScope bl(main->behaviorLock);
00221 PROFSECTION("GotImage()",*mainProfiler);
00222
00223 try {
00224 BufferedImageGenerator::ImageSource& img=main->img;
00225
00226 unsigned int verbose;
00227 unsigned int sn;
00228 string file;
00229 unsigned int payload;
00230 int l;
00231 char* buf=LoadDataThread::deserializeHeader(msg->Base(),msg->Size(),&verbose,&sn,&file,NULL,&payload);
00232 unsigned int remain=payload;
00233 if(verbose>=1 && sn-main->lastVisionSN!=1)
00234 cout << "Main dropped " << (sn-main->lastVisionSN-1) << " camera frame(s)" << endl;
00235 main->lastVisionSN=sn;
00236 if(verbose>=3 && remain==0)
00237 cout << "Main received image heartbeat at " << get_time() << endl;
00238 else if(verbose>=2 && remain!=0)
00239 cout << "Main received image data \"" << file << "\" at " << get_time() << endl;
00240
00241 img.frameIndex=sn;
00242 if(remain==0) {
00243 if(img.width==0 || img.height==0 || img.img==NULL)
00244 return true;
00245 } else {
00246 LoadSave::decodeIncT(l,buf,remain);
00247 LoadSave::decodeIncT(img.width,buf,remain);
00248 LoadSave::decodeIncT(img.height,buf,remain);
00249 LoadSave::decodeIncT(img.channels,buf,remain);
00250 if(l!=0) {
00251 img.layer = (l<0)?ProjectInterface::defRawCameraGenerator->getNumLayers()+l:l-1;
00252 } else {
00253
00254
00255 float fullRes=sqrt(CameraResolutionX*CameraResolutionY);
00256 float givenRes=sqrt(img.width*img.height);
00257 if(givenRes==0) {
00258 cerr << "Main received empty image!" << endl;
00259 return true;
00260 } else {
00261 float ratio=log2f(givenRes/fullRes);
00262 int layerOff=static_cast<int>(rintf(ratio));
00263 int tgtLayer=static_cast<int>(ProjectInterface::fullLayer)+layerOff;
00264 if(tgtLayer<0)
00265 img.layer=0;
00266 else if(ProjectInterface::defRawCameraGenerator!=NULL && static_cast<unsigned int>(tgtLayer)>=ProjectInterface::defRawCameraGenerator->getNumLayers())
00267 img.layer=ProjectInterface::defRawCameraGenerator->getNumLayers()-1;
00268 else
00269 img.layer=tgtLayer;
00270 if(static_cast<unsigned int>(tgtLayer)!=img.layer)
00271 cerr << "Image dimensions of " << img.width << "x" << img.height << " are well beyond the available resolution layers (full is " << CameraResolutionX << "x" << CameraResolutionY << ")" << endl;
00272 }
00273 }
00274 img.img=reinterpret_cast<unsigned char*>(buf);
00275 msg->AddReference();
00276 if(main->curimgregion!=NULL)
00277 main->curimgregion->RemoveReference();
00278 main->curimgregion=msg;
00279 }
00280 DataEvent<BufferedImageGenerator::ImageSource> dev(img,EventBase::visOFbkEGID,0,EventBase::activateETID);
00281 erouter->postEvent(dev);
00282 dev.setTypeID(EventBase::statusETID);
00283 erouter->postEvent(dev);
00284 dev.setTypeID(EventBase::deactivateETID);
00285 erouter->postEvent(dev);
00286 } catch(const std::exception& ex) {
00287 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during camera frame processing",&ex))
00288 throw;
00289 } catch(...) {
00290 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during camera frame processing",NULL))
00291 throw;
00292 }
00293 try {
00294 erouter->processTimers();
00295 } catch(const std::exception& ex) {
00296 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during timer processing",&ex))
00297 throw;
00298 } catch(...) {
00299 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during timer processing",NULL))
00300 throw;
00301 }
00302 if(globals->setNextTimer(erouter->getNextTimer()))
00303 main->timerExec->reset();
00304 return true;
00305 }
00306
00307 bool Main::gotSensors(RCRegion* msg) {
00308 Main * main=dynamic_cast<Main*>(Process::getCurrent());
00309 ASSERTRETVAL(main!=NULL,"gotSensors, but not within Main process!",true);
00310
00311 PROFSECTION("GotSensorFrame()",*mainProfiler);
00312 MarkScope l(main->behaviorLock);
00313
00314 string exceptionPhase="sensor";
00315 try {
00316 unsigned int orgSN=main->lastSensorSN;
00317 WorldStatePool::UpdateInfo * info=main->worldstatepool->isUnread(*msg,globals->motion.frameNumber,main->lastSensorSN,false,globals->motion.override);
00318 int dif=0;
00319 if(info!=NULL) {
00320 bool generateFeedback=(globals->motion.hasUnprovidedOutput() || globals->motion.override) && globals->motion.feedbackDelay>=0;
00321 EntryPoint::WorldStateWrite wsw(info->frameNumber);
00322 MarkScope lw(main->behaviorLock, wsw);
00323 if(state!=NULL && wsw.frame==globals->motion.frameNumber) {
00324
00325
00326 if(main->worldstatepool->read(*info,wsw,generateFeedback,globals->motion.zeroPIDFeedback,NULL)) {
00327 if(wsw.frame-main->lastSensorSN!=1 && info->verbose>=1)
00328 cout << ProcessID::getIDStr() << " dropped " << (wsw.frame-main->lastSensorSN-1) << " sensor frame(s)" << endl;
00329 main->lastSensorSN=wsw.frame;
00330 }
00331 if(wsw.getComplete()) {
00332 dif=state->lastSensorUpdateTime - main->lastSensorUpdateTime;
00333 ASSERT(dif>=0,"sensor update time is negative? " << dif);
00334 main->lastSensorUpdateTime=state->lastSensorUpdateTime;
00335 erouter->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSrcID::UpdatedSID",1);
00336 exceptionPhase="timer";
00337 erouter->processTimers();
00338 }
00339 }
00340 } else if(orgSN!=main->lastSensorSN) {
00341
00342 l.reset();
00343 dif=state->lastSensorUpdateTime - main->lastSensorUpdateTime;
00344 ASSERT(dif>=0,"sensor update time is negative? " << dif);
00345 main->lastSensorUpdateTime=state->lastSensorUpdateTime;
00346 erouter->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSrcID::UpdatedSID",1);
00347 exceptionPhase="timer";
00348 erouter->processTimers();
00349 }
00350 if(dif==0) {
00351 exceptionPhase="timer";
00352 erouter->processTimers();
00353 }
00354 } catch(const std::exception& ex) {
00355 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,("Occurred during "+exceptionPhase+" processing").c_str(),&ex))
00356 throw;
00357 } catch(...) {
00358 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,("Occurred during "+exceptionPhase+" processing").c_str(),NULL))
00359 throw;
00360 }
00361 if(globals->setNextTimer(erouter->getNextTimer()))
00362 main->timerExec->reset();
00363 return true;
00364 }
00365
00366 bool Main::gotEvent(RCRegion* msg) {
00367 if(msg==NULL)
00368 return true;
00369 Main * main=dynamic_cast<Main*>(Process::getCurrent());
00370 ASSERTRETVAL(main!=NULL,"gotEvent, but not within Main process!",true);
00371 MarkScope l(main->behaviorLock);
00372 EventBase* evt=NULL;
00373 try {
00374 evt=EventTranslator::decodeEvent(msg->Base(),msg->Size());
00375 if(evt==NULL) {
00376 cerr << "ERROR: Main::gotEvent() failed to decode message" << endl;
00377 return true;
00378 }
00379 if(evt->getGeneratorID()==EventBase::sensorEGID)
00380 main->lastSensorUpdateTime=evt->getTimeStamp();
00381 erouter->postEvent(*evt);
00382 } catch(const std::exception& ex) {
00383 std::string emsg("Occurred during inter-process event processing");
00384 if(evt!=NULL)
00385 emsg+=": "+evt->getName();
00386 delete evt;
00387 evt=NULL;
00388 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,emsg.c_str(),&ex))
00389 throw;
00390 } catch(...) {
00391 std::string emsg("Occurred during inter-process event processing");
00392 if(evt!=NULL)
00393 emsg+=": "+evt->getName();
00394 delete evt;
00395 evt=NULL;
00396 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,emsg.c_str(),NULL))
00397 throw;
00398 }
00399 delete evt;
00400 evt=NULL;
00401 try {
00402 erouter->processTimers();
00403 } catch(const std::exception& ex) {
00404 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during timer processing",&ex))
00405 throw;
00406 } catch(...) {
00407 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during timer processing",NULL))
00408 throw;
00409 }
00410 if(globals->setNextTimer(erouter->getNextTimer()))
00411 main->timerExec->reset();
00412 return true;
00413 }
00414
00415 bool Main::gotTimer(RCRegion* ) {
00416 Main * main=dynamic_cast<Main*>(Process::getCurrent());
00417 ASSERTRETVAL(main!=NULL,"gotTimer, but not within Main process!",true);
00418
00419 ASSERTRETVAL(main->timerExec!=NULL,"timerExec thread is NULL when timer wakeup received",true);
00420 main->timerExec->reset();
00421
00422 if(globals->timeScale<=0) {
00423 MarkScope bl(main->behaviorLock);
00424 try {
00425 erouter->processTimers();
00426 } catch(const std::exception& ex) {
00427 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during timer processing",&ex))
00428 throw;
00429 } catch(...) {
00430 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during timer processing",NULL))
00431 throw;
00432 }
00433 globals->setNextTimer(erouter->getNextTimer());
00434
00435 }
00436 return true;
00437 }
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449