Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Simulator.cc

Go to the documentation of this file.
00001 #include "Simulator.h"
00002 #include "Main.h"
00003 #include "Motion.h"
00004 #include "sim.h"
00005 #include "Shared/string_util.h"
00006 #include "Shared/RobotInfo.h"
00007 #include "SimConfig.h"
00008 #include "Shared/debuget.h"
00009 #include "Shared/MarkScope.h"
00010 #include "IPC/MessageReceiver.h"
00011 #include "IPC/RegionRegistry.h"
00012 #include "local/DataSources/FileSystemDataSource.h"
00013 #include "local/DataSources/FileSystemImageSource.h"
00014 #include "local/CommPort.h"
00015 #include "local/DeviceDriver.h"
00016 
00017 #include <iostream>
00018 #include <libxml/tree.h>
00019 
00020 #ifndef DISABLE_READLINE
00021 #  include <readline/readline.h>
00022 #  include <readline/history.h>
00023 #endif
00024 
00025 using namespace std;
00026 
00027 const float Simulator::avgSpeedupGamma=.99;
00028 
00029 Simulator* Simulator::theSim=NULL;
00030 std::set<MotionHook*> Simulator::motionHooks;
00031 
00032 Simulator::Simulator()
00033 : Process(getID(),getClassName()), plist::PrimitiveListener(), plist::CollectionListener(), MessageQueueStatusThread::StatusListener(), frameCounter(), cmdThread(),
00034 cameraQueue(ipc_setup->registerRegion(Simulator::getCameraQueueID(),sizeof(sim::CameraQueue_t))),
00035 sensorQueue(ipc_setup->registerRegion(Simulator::getSensorQueueID(),sizeof(sim::SensorQueue_t))),
00036 timerWakeup(ipc_setup->registerRegion(Simulator::getTimerWakeupID(),sizeof(sim::TimerWakeup_t))),
00037 motionWakeup(ipc_setup->registerRegion(Simulator::getMotionWakeupID(),sizeof(sim::MotionWakeup_t))),
00038 statusRequest(ipc_setup->registerRegion(Simulator::getStatusRequestID(),sizeof(sim::StatusRequest_t))),
00039 events(ipc_setup->registerRegion(Main::getEventsID(),sizeof(sim::EventQueue_t))),
00040 motionout(ipc_setup->registerRegion(Motion::getMotionOutputID(),sizeof(sim::MotionOutput_t))),
00041 commandQueue(ipc_setup->registerRegion(Simulator::getCommandQueueID(),sizeof(CommandQueue_t))),
00042 cameraStatus(*cameraQueue), sensorStatus(*sensorQueue), timerStatus(*timerWakeup), motionStatus(*motionWakeup), eventsStatus(), commandrecv(NULL), motionrecv(NULL),
00043 vision(&DeviceDriver::getImageSources,30, *cameraQueue),
00044 sensors(&DeviceDriver::getSensorSources,1000.f/NumFrames/FrameTime, *sensorQueue),
00045 frameTimes(), runSpeed(1), lastTimeScale(0), step(STEP_NONE), waitingSteps(0), curLevel(SharedGlobals::CONSTRUCTING),
00046 fullspeedWallStart(), fullspeedSimStart(), lastFrameWallStart(), avgWallTime(), avgSimTime(),
00047 simLock()
00048 {
00049   theSim=this;
00050   new (&(*cameraQueue)) sim::CameraQueue_t;
00051   new (&(*sensorQueue)) sim::SensorQueue_t;
00052   new (&(*timerWakeup)) sim::TimerWakeup_t;
00053   new (&(*motionWakeup)) sim::MotionWakeup_t;
00054   new (&(*statusRequest)) sim::StatusRequest_t;
00055   new (&(*commandQueue)) CommandQueue_t;
00056   statusRequest->setOverflowPolicy(MessageQueueBase::WAIT);
00057   commandQueue->setOverflowPolicy(MessageQueueBase::WAIT);
00058   
00059   DeviceDriver::getRegistry().addCollectionListener(this);
00060   
00061   /* Since we do a two stage loading (some stuff in sim.cc launcher, then some command line
00062    *  stuff, then the rest of it now that we're loaded), this is a little tricker than it normally would be. */
00063   // don't remove entries from parse tree, we're going to be adding more and want their values...
00064   sim::config.setSavePolicy(plist::Collection::UNION);
00065   // write out the current values for entries we do have, which may have been modified since originally loaded
00066   sim::config.writeParseTree();
00067   // now add the rest of our entries (could just get away with addEntry()'s, but this function is a little more robust)
00068   replaceEntry("CommPorts",CommPort::getRegistry(),"Communication portals for use by device drivers");
00069   replaceEntry("Drivers",DeviceDriver::getRegistry(),"Settings for device drivers");
00070   replaceEntry("Sensors",sensors,"Settings for the loading of sensor values");
00071   replaceEntry("Vision",vision,"Settings for the loading of camera frames");
00072   // now we're done adding entries, so if there's anything extra in the file, make note of it
00073   sim::config.setUnusedWarning(true);
00074   // reload from parse tree to get values for these new entries
00075   sim::config.readParseTree();
00076   // any future saves should be strict and remove those unused values (...maybe?)
00077   sim::config.setSavePolicy(plist::Collection::SYNC);
00078   
00079   motionWakeup->setOverflowPolicy(MessageQueueBase::DROP_OLDEST);
00080   timerWakeup->setOverflowPolicy(MessageQueueBase::DROP_OLDEST);
00081   cameraQueue->setOverflowPolicy(MessageQueueBase::DROP_OLDEST);
00082   sensorQueue->setOverflowPolicy(MessageQueueBase::DROP_OLDEST);
00083   sensorQueue->addMessageFilter(frameCounter);
00084   
00085   //handle arguments passed from command line
00086   vector<vector<string> > delayed; // will delay setting data queue sources until after other arguments
00087   for(unsigned int i=0; i<sim::cmdlineArgs.size(); i++) {
00088     if(sim::cmdlineArgs[i].find(".Source=")==string::npos) {
00089       if(!processCommand(sim::cmdlineArgs[i],false))
00090         cerr << "Occurred while processing " << sim::cmdlineArgs[i] << endl;
00091     } else {
00092       vector<string> setarg;
00093       setarg.push_back("set");
00094       setarg.push_back(sim::cmdlineArgs[i]);
00095       delayed.push_back(setarg); // delay setting of source until after options like Frozen or Speed have been set
00096     }
00097   }
00098   for(unsigned int i=0; i<delayed.size(); i++) {
00099     //this avoids dropping initial frame(s)
00100     //if we had set the source before freezing or pausing, might've dropped something between them
00101     if(!cmdSet(delayed[i]))
00102       cerr << "Occurred while processing " << delayed[i][1] << endl;
00103   }
00104   
00105   // if we were supposed to start up paused, reset clock now that we've set the speed
00106   if(globals->timeScale<=0)
00107     globals->resetBootTime();
00108 
00109   globals->timeScale.addPrimitiveListener(this);
00110   globals->motion.verbose.addPrimitiveListener(this);
00111   
00112   cmdThread.start();
00113   
00114   if(sim::config.tgtRunlevel>SharedGlobals::RUNNING)
00115     globals->signalShutdown();
00116   processRunlevel(SharedGlobals::CONSTRUCTING);
00117 }
00118 
00119 Simulator::~Simulator() {
00120   DeviceDriver::getRegistry().clear();
00121   CommPort::getRegistry().clear();
00122   DeviceDriver::getRegistry().removeCollectionListener(this);
00123   globals->timeScale.removePrimitiveListener(this);
00124   globals->motion.verbose.removePrimitiveListener(this);
00125   processRunlevel(SharedGlobals::DESTRUCTING);
00126   //::close(STDIN_FILENO); // seems to prevent proper terminal reset on exit...
00127 }
00128 
00129 void Simulator::DoStart() {
00130   Process::DoStart();
00131   eventsStatus.setMessageQueue(*events);
00132   commandrecv = new MessageReceiver(*commandQueue, gotCommand);
00133   class : public Thread {
00134   public:
00135     virtual void* run() { Simulator::setMotionStarting(); return NULL; }
00136   } doStartThread;
00137   cmdThread.runInitThread(doStartThread);
00138   processRunlevel(SharedGlobals::STARTING);
00139 }
00140 
00141 void Simulator::run() {
00142   for(unsigned int i=0; i<ProcessID::NumProcesses; ++i)
00143     cout << globals->processNames[i] << " pid=" << globals->pids[i] << ";  ";
00144   cout << endl;
00145   if(globals->timeScale!=0)
00146     runSpeed=globals->timeScale;
00147 
00148   motionrecv = new MessageReceiver(*motionout, gotMotion);
00149   
00150   DataSource::setNeedsSensor(globals->waitForSensors);
00151   resetSpeedMode();
00152   if(globals->timeScale<0)
00153     incrementTime();
00154   
00155   if(globals->waitForSensors) {
00156     if(sensors.source.size()==0) {
00157       cout << "Ignoring WaitForSensors flag because Sensors has no source" << endl;
00158     } else {
00159       if(DeviceDriver::getRegistry().getInstance(sensors.source)==NULL) {
00160         cout << "Ignoring WaitForSensors flag because Sensors source is invalid" << endl;
00161       } else {        
00162         class : public Thread {
00163         public:
00164           virtual void* run() { globals->waitSensors(); return NULL; }
00165         } waitSensorsThread;
00166         cmdThread.runInitThread(waitSensorsThread);
00167       }
00168      }
00169   }
00170   DataSource::setNeedsSensor(false);
00171 
00172   if(sim::config.tgtRunlevel==SharedGlobals::RUNNING)
00173     Process::run();
00174 
00175   sensors.source="";
00176   vision.source="";
00177   if(sensors.isStarted())
00178     sensors.stop();
00179   if(vision.isStarted())
00180     vision.stop();
00181   
00182   if(globals->timeScale<0) {
00183     motionStatus.removeStatusListener(this);
00184     timerStatus.removeStatusListener(this);
00185     sensorStatus.removeStatusListener(this);
00186     cameraStatus.removeStatusListener(this);
00187   }
00188   globals->signalShutdown();
00189 }
00190 
00191 void Simulator::plistValueChanged(const plist::PrimitiveBase& pl) {
00192   MarkScope l(simLock);
00193   if(&pl==&globals->timeScale) {
00194     get_time(); // force SharedGlobals to notice the change and update its state
00195     resetSpeedMode();
00196     if(globals->timeScale<0)
00197       incrementTime();
00198     timerWakeup->sendMessage(NULL);
00199     motionWakeup->sendMessage(NULL);
00200   } else if(&pl==&globals->motion.verbose) {
00201     for(std::set<MotionHook*>::iterator it=motionHooks.begin(); it!=motionHooks.end(); ++it)
00202       (*it)->setMotionHookVerbose(globals->motion.verbose);
00203   } else {
00204     cerr << "WARNING: Simulator got a plistValueChanged for an unknown plist primitive";
00205   }
00206 }
00207 void Simulator::plistCollectionEntryAdded(plist::Collection& col, plist::ObjectBase& primitive) {
00208   MarkScope l(simLock);
00209   if(&col==&DeviceDriver::getRegistry()) {
00210     if(DeviceDriver * d=dynamic_cast<DeviceDriver*>(&primitive)) {
00211       if(MotionHook * mh = d->getMotionSink()) {
00212         mh->setMotionHookVerbose(globals->motion.verbose);
00213         motionHooks.insert(mh);
00214         if(curLevel==SharedGlobals::RUNNING) {
00215           mh->motionStarting();
00216           if(globals->timeScale>0)
00217             mh->enteringRealtime();
00218         }
00219       }
00220     } else {
00221       cerr << "WARNING: Simulator got a plistCollectionEntryAdded for an unknown primitive type";
00222     }
00223   } else {
00224     cerr << "WARNING: Simulator got a plistCollectionEntryAdded for an unknown plist collection";
00225   }
00226 }
00227 void Simulator::plistCollectionEntryRemoved(plist::Collection& col, plist::ObjectBase& primitive) {
00228   MarkScope l(simLock);
00229   if(&col==&DeviceDriver::getRegistry()) {
00230     if(DeviceDriver * d=dynamic_cast<DeviceDriver*>(&primitive)) {
00231       motionHooks.erase(d->getMotionSink());
00232     } else {
00233       cerr << "WARNING: Simulator got a plistCollectionEntryRemoved for an unknown primitive type";
00234     }
00235   } else {
00236     cerr << "WARNING: Simulator got a plistCollectionEntryRemoved for an unknown plist collection";
00237   }
00238 }
00239 void Simulator::plistCollectionEntriesChanged(plist::Collection& col) {
00240   MarkScope l(simLock);
00241   if(&col==&DeviceDriver::getRegistry()) {
00242     std::set<MotionHook*> oldmh;
00243     motionHooks.swap(oldmh);
00244     for(DeviceDriver::registry_t::const_iterator it=DeviceDriver::getRegistry().begin(); it!=DeviceDriver::getRegistry().end(); ++it) {
00245       if(DeviceDriver * d=DeviceDriver::getRegistry().getInstance(it->first)) {
00246         if(MotionHook * mh = d->getMotionSink()) {
00247           mh->setMotionHookVerbose(globals->motion.verbose);
00248           motionHooks.insert(mh);
00249           if(oldmh.find(mh)==oldmh.end() && curLevel==SharedGlobals::RUNNING) {
00250             mh->motionStarting();
00251             if(globals->timeScale>0)
00252               mh->enteringRealtime();
00253           }
00254         }
00255       } else {
00256         cerr << "WARNING: In Simulator::plistCollectionEntriesChanged, driver " << it->first << " does not correspond to a known instance" << endl;
00257       }
00258     }
00259   } else {
00260     cerr << "WARNING: Simulator got a plistCollectionEntriesChanged for an unknown plist collection";
00261   }
00262 }
00263 
00264 void Simulator::messagesRead(MessageQueueBase& mq, unsigned int /*n*/) {
00265   MarkScope l(simLock);
00266   if(globals->timeScale<0) {
00267     //clear corresponding bit in waitingSteps
00268      if(&mq==&(*cameraQueue)) {
00269       //cout << "Camera read, ";
00270       waitingSteps&=~(1<<STEP_CAMERA);
00271     } else if(&mq==&(*sensorQueue)) {
00272       //cout << "Sensor read, ";
00273       waitingSteps&=~(1<<STEP_SENSOR);
00274     } else if(&mq==&(*timerWakeup)) {
00275       //cout << "Timer read, ";
00276       waitingSteps&=~(1<<STEP_TIMER);
00277     } else if(&mq==&(*motionWakeup)) {
00278       //cout << "Motion read, ";
00279       waitingSteps&=~(1<<STEP_MOTION);
00280     } else if(&mq==&(*events)) {
00281       //cout << "Main read event queue (" << events->getMessagesUnread() << "remain), ";
00282       // nothing to do, just waiting for main to catch up before incrementing
00283     } else {
00284       cout << "Unknown message base read (either you meant to add some code to Simulator::messagesRead, or why did you bother to register a listener?)" << endl;
00285     }
00286     //cout << " waiting " << waitingSteps << " events " << events->getMessagesUnread() << endl;
00287     
00288     if(waitingSteps==0 && events->getMessagesUnread()==0) //if that was the last one we were waiting for -- go for the next!
00289       incrementTime();
00290   }
00291 }
00292 
00293 void Simulator::sendCommand(const std::string& cmd) {
00294   static unsigned int cmdSN=0;
00295   char msgname[30];
00296   snprintf(msgname,30,"SimCommand.%d.%d",ProcessID::getID(),cmdSN++);
00297   RCRegion * msg = new RCRegion(msgname,cmd.size());
00298   strcpy(msg->Base(),cmd.c_str());
00299   SharedObject<CommandQueue_t> commandQ(ipc_setup->registerRegion(Simulator::getCommandQueueID(),sizeof(CommandQueue_t)));
00300   commandQ->sendMessage(msg,true);
00301 }
00302 
00303 void Simulator::setMotionStarting() {
00304   // don't need lock, this is only called during single-threaded operation (DoStart())
00305   //MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00306   for(std::set<MotionHook*>::iterator it=theSim->motionHooks.begin(); it!=theSim->motionHooks.end(); ++it)
00307     (*it)->motionStarting();
00308 }
00309 void Simulator::setMotionStopping() {
00310   // don't need lock, this is only called during single-threaded operation (DoStop())
00311   //MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00312   for(std::set<MotionHook*>::iterator it=theSim->motionHooks.begin(); it!=theSim->motionHooks.end(); ++it)
00313     (*it)->motionStopping();
00314 }
00315 void Simulator::updateMotion(const float outputs[][NumOutputs]) {
00316   MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00317   for(std::set<MotionHook*>::iterator it=theSim->motionHooks.begin(); it!=theSim->motionHooks.end(); ++it)
00318     (*it)->motionCheck(outputs);
00319 }
00320 void Simulator::setMotionLeavingRealtime(bool isFullSpeed) {
00321   MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00322   for(std::set<MotionHook*>::iterator it=theSim->motionHooks.begin(); it!=theSim->motionHooks.end(); ++it)
00323     (*it)->leavingRealtime(isFullSpeed);
00324 }
00325 void Simulator::setMotionEnteringRealtime() {
00326   MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00327   for(std::set<MotionHook*>::iterator it=theSim->motionHooks.begin(); it!=theSim->motionHooks.end(); ++it)
00328     (*it)->enteringRealtime();
00329 }
00330 
00331 
00332 void Simulator::DoStop() {
00333   if(sensors.isStarted() || vision.isStarted())
00334     sleep(1);
00335   if(sensors.isStarted() || vision.isStarted())
00336     sleep(1);
00337   if(sensors.isStarted())
00338     sensors.kill();
00339   if(vision.isStarted())
00340     vision.kill();
00341   commandrecv->finish();
00342   delete commandrecv;
00343   commandrecv=NULL;
00344   motionrecv->finish();
00345   delete motionrecv;
00346   motionrecv=NULL;
00347   setMotionStopping();
00348   processRunlevel(SharedGlobals::STOPPING);
00349   Process::DoStop();
00350 }
00351 
00352 
00353 void* Simulator::CommandThread::run() {
00354   Simulator * simp=dynamic_cast<Simulator*>(Process::getCurrent());
00355   ASSERTRETVAL(simp!=NULL,"CommandThread not in Simulator!",NULL);
00356   string prompt; // initially prompt is empty because it will be buried in startup text anyway
00357   // we'll display another prompt at the end of launch (or if user hits enter)
00358   while(true) {
00359     testCancel();
00360     string line;
00361 #ifndef DISABLE_READLINE
00362     char* readin=readline(prompt.c_str());
00363     if(readin==NULL) {
00364       cout << endl;
00365       simp->cmdQuit(vector<string>());
00366       break;
00367     }
00368     line=readin;
00369     free(readin);
00370 #else
00371     cout << prompt << flush;
00372     getline(cin,line);
00373     if(!cin) {
00374       cout << endl;
00375       simp->cmdQuit(vector<string>());
00376       break;
00377     }
00378 #endif
00379     simp->processCommand(line,true);
00380     prompt = sim::config.cmdPrompt;
00381   }
00382   return NULL;
00383 }
00384 
00385 void Simulator::CommandThread::cancelled() {
00386 #ifndef DISABLE_READLINE
00387   rl_reset_terminal(NULL);
00388 #endif
00389 }
00390 
00391 void Simulator::CommandThread::runInitThread(Thread& th) {
00392   initThread=&th;
00393   th.start();
00394   th.join();
00395   initThread=NULL;
00396 }
00397 
00398 void Simulator::resetSpeedMode() {
00399   if(globals->timeScale<=0) {
00400     if(vision.isStarted()) {
00401       vision.stop();
00402       //vision.join(); // can't join because runfor/runto pause might be triggered within LoadFileThread's get_time() call
00403     }
00404     if(sensors.isStarted()) {
00405       sensors.stop();
00406       //sensors.join(); // can't join because runfor/runto pause might be triggered within LoadFileThread's get_time() call
00407     }
00408   } else {
00409     if(sim::config.tgtRunlevel==SharedGlobals::RUNNING) { 
00410       vision.start();
00411       sensors.start();
00412     }
00413   }
00414   if(lastTimeScale>0 && globals->timeScale<=0) {
00415     setMotionLeavingRealtime(globals->timeScale<0);
00416   } else if(lastTimeScale<=0 && globals->timeScale>0) {
00417     setMotionEnteringRealtime();
00418   }
00419   if(globals->timeScale<0) {
00420     cameraQueue->setOverflowPolicy(MessageQueueBase::WAIT);
00421     sensorQueue->setOverflowPolicy(MessageQueueBase::WAIT);
00422     // these status listener calls check to prevent duplicate listeners
00423     cameraStatus.addStatusListener(this);
00424     sensorStatus.addStatusListener(this);
00425     timerStatus.addStatusListener(this);
00426     motionStatus.addStatusListener(this);
00427     eventsStatus.addStatusListener(this);
00428     fullspeedWallStart.Set();
00429     fullspeedSimStart=globals->simulatorTime;
00430     lastFrameWallStart.Set();
00431     avgWallTime=avgSimTime=0;
00432   } else {
00433     cameraQueue->setOverflowPolicy(MessageQueueBase::DROP_OLDEST);
00434     sensorQueue->setOverflowPolicy(MessageQueueBase::DROP_OLDEST);
00435     eventsStatus.removeStatusListener(this);
00436     motionStatus.removeStatusListener(this);
00437     timerStatus.removeStatusListener(this);
00438     sensorStatus.removeStatusListener(this);
00439     cameraStatus.removeStatusListener(this);
00440   }
00441   if(globals->timeScale==0)
00442     globals->setAutoPauseTime(-1U);
00443   lastTimeScale=globals->timeScale;
00444 }
00445 
00446 void Simulator::replaceEntry(const std::string& name, plist::Dictionary& d, const std::string& comment) {
00447   plist::Dictionary::const_iterator it = sim::config.findEntry(name);
00448   if(it==sim::config.end()) {
00449     sim::config.addEntry(name,d,comment);
00450   } else {
00451     d.set(*it->second);
00452     sim::config.setEntry(name,d);
00453     sim::config.setComment(name,comment);
00454   }
00455 }
00456 
00457 void Simulator::incrementTime() {
00458   MarkScope l(simLock);
00459   waitingSteps=getNextFrame();
00460   if(waitingSteps==0)
00461     return;
00462   unsigned int next=*frameTimes.begin();
00463   if(next>globals->simulatorTime) {
00464     unsigned int adv=next-globals->simulatorTime;
00465     avgWallTime=avgWallTime*avgSpeedupGamma + lastFrameWallStart.Age().Value()*(1-avgSpeedupGamma);
00466     avgSimTime=avgSimTime*avgSpeedupGamma + adv*(1-avgSpeedupGamma);
00467     lastFrameWallStart.Set();
00468     //cout << "inc " << (avgSimTime/avgWallTime/1000) << " " << waitingSteps << endl;
00469     globals->simulatorTime=next;
00470   }
00471   if(waitingSteps & (1<<STEP_CAMERA))
00472     vision.advanceFrame(false);
00473   if(waitingSteps & (1<<STEP_SENSOR))
00474     sensors.advanceFrame(false);
00475   if(waitingSteps & (1<<STEP_TIMER))
00476     timerWakeup->sendMessage(NULL);
00477   if(waitingSteps & (1<<STEP_MOTION))
00478     motionWakeup->sendMessage(NULL);
00479   if(globals->getAutoPauseTime()<=globals->simulatorTime || (1<<step) & waitingSteps) {
00480     //that was the step we were waiting for, pause sim
00481     globals->timeScale=0;
00482     step=STEP_NONE;
00483     globals->setAutoPauseTime(-1U);
00484   }
00485 }
00486 
00487 unsigned int Simulator::getNextFrame() {
00488   frameTimes.clear();
00489   /*set<unsigned int>::iterator past=frameTimes.begin();
00490   while(past!=frameTimes.end() && *past<=globals->simulatorTime)
00491   past++;
00492   frameTimes.erase(frameTimes.begin(),past);*/
00493   unsigned int vis = vision.frozen && !vision.heartbeat ? -1U : vision.nextTimestamp();
00494   frameTimes.insert(vis);
00495   unsigned int sen = sensors.frozen && !sensors.heartbeat ? -1U : sensors.nextTimestamp();
00496   frameTimes.insert(sen);
00497   unsigned int tim=globals->getNextTimer();
00498   frameTimes.insert(tim);
00499   unsigned int mot=globals->getNextMotion();
00500   frameTimes.insert(mot);
00501   unsigned int next=*frameTimes.begin();
00502   //cout << "Testing: " << globals->simulatorTime << " => next camera: "<< vis << " next sensor: " << sen << " next timer: " << tim << " next motion: " << mot << " => " << next << endl;
00503   unsigned int steps=0;
00504   if(next!=-1U) {
00505     if(next==vis) {
00506       steps |= 1<<STEP_CAMERA;
00507     }
00508     if(next==sen) {
00509       steps |= 1<<STEP_SENSOR;
00510     }
00511     if(next==tim) {
00512       steps |= 1<<STEP_TIMER;
00513     }
00514     if(next==mot) {
00515       steps |= 1<<STEP_MOTION;
00516     }
00517   }
00518   return steps;
00519 }
00520 
00521 void Simulator::processRunlevel(SharedGlobals::runlevel_t curRunLevel) {
00522   curLevel=curRunLevel;
00523   if(sim::config.tgtRunlevel==curLevel && (!globals->isShutdown() || curLevel>SharedGlobals::RUNNING))
00524     cout << sim::config.cmdPrompt << flush;
00525   while(sim::config.tgtRunlevel==curLevel && (!globals->isShutdown() || curLevel>SharedGlobals::RUNNING)) {
00526     usleep(500000); // recheck status every half second
00527   }
00528 }
00529 
00530 bool Simulator::processCommand(const std::string& line, bool addToHistory) {
00531   vector<string> args;
00532   vector<unsigned int> offs;
00533   if(!string_util::parseArgs(line,args,offs)) {
00534     cerr << "Mismatched quotes" << endl;
00535     return false;
00536   }
00537   if(args.size()==0)
00538     return true;
00539 #ifndef DISABLE_READLINE
00540   /*    if(current_history()==NULL)
00541     cout << "current_history()==NULL" << endl;
00542   else if(current_history()->line==NULL)
00543     cout << "line == NULL" << endl;
00544   else if(line!=current_history()->line)
00545     cout << "line is different" << endl;
00546   else {
00547     cout << "not added" << endl;
00548     cout << "new line: " << line << endl;
00549     cout << "old line: " << current_history()->line << endl;
00550   }
00551   if(history_get(-1)==NULL)
00552     cout << "history_get(0)==NULL" << endl;
00553   else if(history_get(-1)->line==NULL)
00554     cout << "line 0 == NULL" << endl;
00555   else {
00556     cout << "line 0: " << history_get(-1)->line << endl;
00557     if(line!=history_get(-1)->line)
00558       cout << "line 0 is different" << endl;
00559     else
00560       cout << "0 not added" << endl;
00561   }
00562   */  
00563   if(addToHistory && (current_history()==NULL || current_history()->line==NULL || line!=current_history()->line))
00564     add_history(line.c_str());
00565 #endif
00566   if(args[0]=="shutdown" || args[0]=="quit" || args[0]=="exit") {
00567     cmdQuit(args);
00568   } else if(args[0]=="load") {
00569     cmdLoad(args);
00570   } else if(args[0]=="save") {
00571     cmdSave(args);
00572   } else if(args[0]=="runlevel") {
00573     cmdRunlevel(args);
00574   } else if(args[0]=="get_time") {
00575     cout << "Current time is " << get_time() << endl;
00576   } else if(args[0]=="set") {
00577     cmdSet(args);
00578   } else if(args[0]=="runto") {
00579     cmdRun(args,false);
00580   } else if(args[0]=="runfor") {
00581     cmdRun(args,true);
00582   } else if(args[0]=="run" || args[0]=="r") {
00583     cmdRun(args);
00584   } else if(args[0]=="pause" || args[0]=="p") {
00585     cmdPause(args);
00586   } else if(args[0]=="help") {
00587     cmdHelp(args);
00588   } else if(args[0]=="step") {
00589     cmdStep(args);
00590   } else if(args[0]=="status") {
00591     cmdStatus(args);
00592   } else if(args[0]=="advance") {
00593     cmdAdvance(args);
00594   } else if(args[0]=="freeze") {
00595     cmdFreeze(true,args);
00596   } else if(args[0]=="unfreeze") {
00597     cmdFreeze(false,args);
00598   } else if(args[0]=="reset") {
00599     cmdReset(args);
00600   } else if(args[0]=="new") {
00601     cmdNew(args);
00602   } else if(args[0]=="delete") {
00603     cmdDelete(args);
00604   } else {
00605     unsigned int i;
00606     for(i=0; i<args.size(); ++i) {
00607       if(args[i].find("=")!=string::npos) {
00608         cmdSet(args);
00609         break;
00610       }
00611     }
00612     if(i==args.size()) {
00613       cout << "Unknown command '" << args[0] << "'" << endl;
00614       return false;
00615     }
00616   }
00617   return true;
00618 }
00619 
00620 bool Simulator::gotCommand(RCRegion* msg) {
00621   Simulator * simp=dynamic_cast<Simulator*>(Process::getCurrent());
00622   ASSERTRETVAL(simp!=NULL,"gotCommand, but not within Simulator process!",true);
00623   simp->processCommand(msg->Base(),false);
00624   return true;
00625 } 
00626 
00627 bool Simulator::gotMotion(RCRegion* msg) {
00628 #ifdef DEBUG
00629   Simulator * simp=dynamic_cast<Simulator*>(Process::getCurrent());
00630   ASSERTRETVAL(simp!=NULL,"gotMotion, but not within Simulator process!",true);
00631 #endif
00632   updateMotion(reinterpret_cast<float(*)[NumOutputs]>(msg->Base()));
00633   return true;
00634 }
00635 
00636 void Simulator::cmdQuit(const std::vector<std::string>& /*args*/) {
00637   sim::config.tgtRunlevel=SharedGlobals::DESTRUCTED;
00638   globals->signalShutdown();
00639   cmdThread.abortInitThread();
00640 }
00641 void Simulator::cmdLoad(const std::vector<std::string>& args) {
00642   if(args.size()>1)
00643     for(unsigned int i=1; i<args.size(); i++) {
00644       cout << "Loading from " << args[i] << "... " << flush;
00645       size_t res=sim::config.loadFile(args[i].c_str());
00646       cout << (res>0 ? "done." : "load failed.") << endl;
00647     }
00648