Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Motion.cc

Go to the documentation of this file.
00001 #include "Motion.h"
00002 #include "Main.h"
00003 #include "SoundPlay.h"
00004 #include "Simulator.h"
00005 #include "SimConfig.h"
00006 #include "Shared/Config.h"
00007 #include "MotionExecThread.h"
00008 
00009 #include "IPC/RegionRegistry.h"
00010 #include "IPC/MessageReceiver.h"
00011 #include "Motion/Kinematics.h"
00012 #include "Wireless/Wireless.h"
00013 #include "Events/EventRouter.h"
00014 #include "Shared/MarkScope.h"
00015 
00016 #include "local/MotionHooks/IPCMotionHook.h"
00017 
00018 using namespace std;
00019 
00020 Motion::Motion()
00021   : Process(getID(),getClassName()),
00022   sounds(ipc_setup->registerRegion(SoundPlay::getSoundPlayID(),sizeof(sim::SoundPlayQueue_t))),
00023   motions(ipc_setup->registerRegion(getMotionCommandID(),sizeof(sim::MotionCommandQueue_t))),
00024   motionout(ipc_setup->registerRegion(getMotionOutputID(),sizeof(sim::MotionOutput_t))),
00025   events(ipc_setup->registerRegion(Main::getEventsID(),sizeof(sim::EventQueue_t))),
00026   sensorFrames(ipc_setup->registerRegion(Simulator::getSensorQueueID(),sizeof(sim::SensorQueue_t))),
00027   statusRequest(ipc_setup->registerRegion(Simulator::getStatusRequestID(),sizeof(sim::StatusRequest_t))),
00028   motionmanager(ipc_setup->registerRegion(getMotionManagerID(),sizeof(MotionManager))),
00029   soundmanager(ipc_setup->registerRegion(SoundPlay::getSoundManagerID(),sizeof(SoundManager))),
00030   worldstatepool(ipc_setup->registerRegion(Main::getWorldStatePoolID(),sizeof(WorldStatePool))),
00031   motionWakeup(ipc_setup->registerRegion(Simulator::getMotionWakeupID(),sizeof(sim::MotionWakeup_t))),
00032   motionProf(ipc_setup->registerRegion(getMotionProfilerID(),sizeof(motionProfiler_t))),
00033   etrans(NULL), sensrecv(NULL), statusrecv(NULL), wakeuprecv(NULL), motionExec(NULL), motionfwd(NULL),
00034   wireless_thread(), motionLock(*worldstatepool), lastSensorSN(-1U)
00035 {
00036   new (&(*motions)) sim::MotionCommandQueue_t;
00037   new (&(*motionout)) sim::MotionOutput_t;
00038   new (&(*motionmanager)) MotionManager;
00039   new (&(*motionProf)) motionProfiler_t;
00040   motman=&(*motionmanager);
00041   motman->InitAccess(*motions,motionLock);
00042   sndman=&(*soundmanager);
00043   state=NULL;
00044   ::motionProfiler=&(*motionProf);
00045   
00046   if(sim::config.multiprocess) {
00047     //Setup wireless
00048     ASSERT(wireless==NULL,"global wireless already initialized before Motion?");
00049     wireless = new Wireless();
00050     sout=wireless->socket(Socket::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*12);
00051     serr=wireless->socket(Socket::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*4);
00052     wireless->setDaemon(sout);
00053     wireless->setDaemon(serr);
00054     serr->setFlushType(Socket::FLUSH_BLOCKING);
00055     sout->setTextForward();
00056     serr->setForward(sout);
00057 
00058     //Setup Kinematics
00059     ASSERT(kine==NULL,"global kine already initialized before Motion?");
00060     kine=new Kinematics();
00061   }
00062 
00063   //EventRouter and Config are set up for all processes by main() before fork
00064 }
00065 
00066 Motion::~Motion() {
00067   MotionManager::setTranslator(NULL);
00068   delete etrans;
00069   etrans=NULL;
00070   if(sim::config.multiprocess) {
00071     delete wireless;
00072     wireless=NULL;
00073     delete kine;
00074     kine=NULL;
00075   }
00076 }
00077 
00078 void Motion::DoStart() {
00079   Process::DoStart();
00080   //These are constructed by other processes, so need to wait
00081   //until the construction runlevel is complete before we access them
00082   sndman->InitAccess(*sounds);
00083   if(!sim::config.multiprocess) {
00084     // don't use our own etrans here, because erouter will delete it for us, don't want a double-delete in our destructor...
00085     EventTranslator * forwardTrans = new IPCEventTranslator(*events);
00086     forwardTrans->setTrapEventValue(true);
00087     erouter->setForwardingAgent(getID(),forwardTrans);
00088     MotionManager::setTranslator(forwardTrans);
00089   } else {
00090     etrans=new IPCEventTranslator(*events);
00091     MotionManager::setTranslator(etrans);
00092     
00093     // Set up Event Translator to trap and send events to main process
00094     //send everything over except erouter events
00095     for(unsigned int i=0; i<EventBase::numEGIDs; i++)
00096       if(i!=EventBase::erouterEGID)
00097         erouter->addTrapper(etrans,static_cast<EventBase::EventGeneratorID_t>(i));
00098     
00099     wireless->listen(sout, config->motion.console_port);
00100     wireless->listen(serr, config->motion.stderr_port);
00101     wireless_thread.start();
00102   }
00103   
00104   sensrecv=new MessageReceiver(*sensorFrames,gotSensors);
00105   statusrecv=new MessageReceiver(*statusRequest,statusReport);
00106   wakeuprecv=new MessageReceiver(*motionWakeup,gotWakeup,false);
00107 
00108   motionExec=new MotionExecThread(motionLock);
00109   if(sim::config.multiprocess) {
00110     Simulator::registerMotionHook(*(motionfwd=new IPCMotionHook(*motionout)));
00111     Simulator::setMotionStarting();
00112     if(globals->timeScale>0)
00113       Simulator::setMotionEnteringRealtime();
00114   }
00115 }
00116 
00117 
00118 void Motion::run() {
00119   if(globals->waitForSensors)
00120     globals->waitSensors();
00121   
00122   // might have shutdown triggered while waiting
00123   // (perhaps device isn't available, user's killing the process...)
00124   if(globals->isShutdown())
00125     return; // skip running altogether
00126   
00127   motionExec->reset(); //starts the thread if appropriate
00128   wakeuprecv->start();
00129   Process::run();
00130   wakeuprecv->stop();
00131 }
00132 
00133 void Motion::DoStop() {
00134   Simulator::deregisterMotionHook(*motionfwd);
00135   delete motionfwd;
00136   
00137   wakeuprecv->finish();
00138   statusrecv->finish();
00139 
00140   delete motionExec;
00141   motionExec=NULL;
00142   delete wakeuprecv;
00143   wakeuprecv=NULL;
00144   delete statusrecv;
00145   statusrecv=NULL;
00146   
00147   sensrecv->finish();
00148   delete sensrecv;
00149   sensrecv=NULL;
00150   
00151   if(sim::config.multiprocess) {
00152     if(globals->timeScale>0)
00153       Simulator::setMotionLeavingRealtime(false);
00154     Simulator::setMotionStopping();
00155     
00156     erouter->removeTrapper(etrans);
00157     delete etrans;
00158     etrans=NULL;
00159     
00160     wireless_thread.stop();
00161     wireless_thread.join();
00162     wireless->setDaemon(sout,false);
00163     wireless->close(sout);
00164     sout=NULL;
00165     wireless->setDaemon(serr,false);
00166     wireless->close(serr);
00167     serr=NULL;
00168   }
00169   motman->RemoveAccess();
00170   
00171   Process::DoStop();
00172 }
00173 
00174 bool Motion::gotWakeup(RCRegion* /*msg*/) {
00175   Motion * motion=dynamic_cast<Motion*>(Process::getCurrent());
00176   ASSERTRETVAL(motion!=NULL,"gotWakeup, but not within motion process!",true);
00177   ASSERTRETVAL(motion->motionExec!=NULL,"motionExec thread is NULL when motion wakeup received",true);
00178   
00179   MarkScope l(motion->motionLock);
00180   if(sim::config.multiprocess) {
00181     if(globals->timeScale<=0 && motion->motionExec->isStarted())
00182       Simulator::setMotionLeavingRealtime(globals->timeScale<0);
00183     else if(globals->timeScale>0 && !motion->motionExec->isStarted())
00184       Simulator::setMotionEnteringRealtime();
00185   }
00186   if(globals->timeScale<=0 && get_time()>=globals->getNextMotion())
00187     motion->motionExec->poll();
00188   motion->motionExec->reset(); //reset will set globals->getNextMotion(), so have to do this after the poll itself
00189   return true;
00190 }
00191 
00192 bool Motion::gotSensors(RCRegion* msg) {
00193   Motion * motion=dynamic_cast<Motion*>(Process::getCurrent());
00194   ASSERTRETVAL(motion!=NULL,"gotSensors, but not within Motion process!",true);
00195   
00196   PROFSECTION("GotSensorFrame()",*motionProfiler);
00197   MarkScope l(motion->motionLock);
00198   
00199   EntryPoint::WorldStateWrite wsw(-1U);
00200   try {
00201     WorldStatePool::UpdateInfo * info=motion->worldstatepool->isUnread(*msg,globals->motion.frameNumber,motion->lastSensorSN,globals->motion.feedbackDelay>=0,globals->motion.override);
00202     int dif=0;
00203     if(info!=NULL) {
00204       bool generateFeedback=(globals->motion.hasUnprovidedOutput() || globals->motion.override) && globals->motion.feedbackDelay>=0;
00205       const PostureEngine* pose = generateFeedback ? &motion->motionExec->getPostureFeedback() : NULL;
00206       wsw.frame=info->frameNumber;
00207       MarkScope lw(motion->motionLock, wsw);
00208       if(state==NULL || wsw.frame<globals->motion.frameNumber) {
00209         //a newer one came in while we were waiting for the lock (we'll report the accumulated drops next time we get one we don't drop)
00210         return true;
00211       }
00212       ASSERT((wsw.getStatus()&WorldStatePool::FEEDBACK_APPLIED)==0,"motion feedback already applied?");
00213       bool completedPartial=!wsw.getComplete() && (wsw.getStatus()&WorldStatePool::SENSORS_APPLIED)!=0;
00214       if(generateFeedback && info->verbose>=2)
00215         cout << "Motion filling in sensor feedback" << endl;
00216       ASSERT(!completedPartial || generateFeedback,"partially complete, yet no feedback?");
00217       if(motion->worldstatepool->read(*info,wsw,generateFeedback,globals->motion.zeroPIDFeedback,pose)) {
00218         if(wsw.frame-motion->lastSensorSN!=1 && info->verbose>=1)
00219           cout << ProcessID::getIDStr() << " dropped " << (wsw.frame-motion->lastSensorSN-1) << " sensor frame(s)" << endl;
00220         motion->lastSensorSN=wsw.frame;
00221       }
00222       if(completedPartial && wsw.getComplete()) { // was partial before, now completed as well
00223         dif=state->lastSensorUpdateTime - wsw.src->lastSensorUpdateTime;
00224         ASSERT(dif>=0,"sensor update time is negative? " << dif << ' ' << state << ' ' << wsw.src);
00225       }
00226     }
00227     if(dif>0) {
00228       // need to notify main that the sensor update it started is now complete
00229       // do it outside the write lock so if main gets this instantly it can use the new data
00230       // some sleight of hand here -- we've previously registered an IPCEventTranslator as an event trapper.  By posting here, the event will be sent to the Main process
00231       erouter->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSrcID::UpdatedSID",1);
00232     }
00233   } catch(const std::exception& ex) {
00234     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during sensor processing",&ex))
00235       throw;
00236   } catch(...) {
00237     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during sensor processing",NULL))
00238       throw;
00239   }
00240   
00241   return true;
00242 }
00243   
00244 
00245 /*! @file
00246  * @brief 
00247  * @author ejt (Creator)
00248  *
00249  * $Author: ejt $
00250  * $Name: tekkotsu-4_0 $
00251  * $Revision: 1.3 $
00252  * $State: Exp $
00253  * $Date: 2007/11/11 23:57:30 $
00254  */
00255 

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