Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Main.cc

Go to the documentation of this file.
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     //Setup wireless
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     //Setup Kinematics
00070     ASSERT(kine==NULL,"global kine already initialized before Main?");
00071     kine=new Kinematics();
00072   }
00073   wireless->setCallbackLock(behaviorLock);
00074 
00075   //EventRouter and Config are set up for all processes by main() before fork
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   //These are constructed by other processes, so need to wait
00092   //until the construction runlevel is complete
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   // might have shutdown triggered while waiting
00127   // (perhaps device isn't available, user's killing the process...)
00128   if(globals->isShutdown())
00129     return; // skip running altogether
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   // this is a bit of a hack, but once we're done launching, display the prompt:
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; // can't do the heartbeat, don't have an initial image to replicate
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         // using "automatic" mode, pick the layer closest to resolution of provided image
00254         // assumes each layer doubles in size, with smallest layer at 0
00255         float fullRes=sqrt(CameraResolutionX*CameraResolutionY); // from RobotInfo
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         // ^-- make sure a newer one didn't come in while we were waiting for the lock
00325         // (if one did, we'll report the accumulated drops next time we get one we don't drop)
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       // received update after sensor frame was already parsed by other process
00342       l.reset(); //update state
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* /*msg*/) {
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     // don't need to reset timerExec (again) because we're only here if speed<=0, in which case timerExec isn't running main->timerExec->reset();
00435   }
00436   return true;
00437 }
00438 
00439 /*! @file
00440  * @brief 
00441  * @author ejt (Creator)
00442  *
00443  * $Author: ejt $
00444  * $Name: tekkotsu-4_0 $
00445  * $Revision: 1.4 $
00446  * $State: Exp $
00447  * $Date: 2007/11/11 23:57:30 $
00448  */
00449 

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