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
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
00059 ASSERT(kine==NULL,"global kine already initialized before Motion?");
00060 kine=new Kinematics();
00061 }
00062
00063
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
00081
00082 sndman->InitAccess(*sounds);
00083 if(!sim::config.multiprocess) {
00084
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
00094
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
00123
00124 if(globals->isShutdown())
00125 return;
00126
00127 motionExec->reset();
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* ) {
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();
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
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()) {
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
00229
00230
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
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255