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   else {
00649     cout << "Loading from " << sim::config.getLastFile() << "... " << flush;
00650     size_t res=sim::config.loadFile(sim::config.getLastFile().c_str());
00651     cout << (res>0 ? "done." : "load failed.") << endl;
00652   }
00653 }
00654 void Simulator::cmdSave(const std::vector<std::string>& args) {
00655   if(args.size()>1)
00656     for(unsigned int i=1; i<args.size(); i++)
00657       sim::config.saveFile(args[i].c_str());
00658   else {
00659     cout << "Saving to " << sim::config.getLastFile() << "... " << flush;
00660     size_t res=sim::config.saveFile(sim::config.getLastFile().c_str());
00661     cout << (res>0 ? "done." : "save failed.") << endl;
00662   }
00663 }
00664 void Simulator::cmdRunlevel(const std::vector<std::string>& args) {
00665   if(args.size()<=1) {
00666     sim::config.tgtRunlevel=static_cast<SharedGlobals::runlevel_t>(sim::config.tgtRunlevel+1);
00667     cout << "Moving to next runlevel: " << SharedGlobals::runlevel_names[sim::config.tgtRunlevel] << endl;
00668   } else {
00669     try {
00670       sim::config.tgtRunlevel=string_util::makeUpper(args[1]);
00671     } catch(...) {
00672       cout << "Invalid runlevel specification.  Try one of:\n\t";
00673       for(unsigned int i=0; i<SharedGlobals::NUM_RUNLEVELS; i++)
00674         cout << i << ' ' << SharedGlobals::runlevel_names[i] << ", ";
00675       cout << "\nCurrently at " << SharedGlobals::runlevel_names[curLevel] << endl;
00676       return;
00677     }
00678     if(sim::config.tgtRunlevel<curLevel) {
00679       sim::config.tgtRunlevel=curLevel;
00680       cout << "Cannot reduce runlevel, currently at " << curLevel << ' ' << SharedGlobals::runlevel_names[curLevel] << "\n\t";
00681       for(unsigned int i=0; i<SharedGlobals::NUM_RUNLEVELS; i++)
00682         cout << i << ' ' << SharedGlobals::runlevel_names[i] << ", ";
00683       cout << endl;
00684       return;
00685     } else if(sim::config.tgtRunlevel==curLevel) {
00686       cout << "Already at " << curLevel << ' ' << SharedGlobals::runlevel_names[curLevel] << "\n\t";
00687       for(unsigned int i=0; i<SharedGlobals::NUM_RUNLEVELS; i++)
00688         cout << i << ' ' << SharedGlobals::runlevel_names[i] << ", ";
00689       cout << endl;
00690       return;
00691     }
00692   }
00693   if(sim::config.tgtRunlevel>SharedGlobals::RUNNING && curLevel<=SharedGlobals::RUNNING)
00694     globals->signalShutdown();
00695 }
00696 bool Simulator::cmdSet(const std::vector<std::string>& args) {
00697   if(args.size()==0 || args[0]=="set" && args.size()==1) {
00698     plist::filteredDisplay(cout,sim::config,"^[^.].*",REG_EXTENDED,3);
00699     return false;
00700   }
00701   string arg;
00702   for(unsigned int i=(args[0]=="set"?1:0); i<args.size(); i++) {
00703     arg+=args[i];
00704     if(i!=args.size()-1)
00705       arg+=" ";
00706   }
00707   if(arg.rfind("=")==string::npos) {
00708     plist::ObjectBase* ob=sim::config.resolveEntry(arg);
00709     if(ob==NULL) {
00710       cout << "'" << arg << "' is unknown" << endl;
00711       return false;
00712     }
00713     plist::filteredDisplay(cout,*ob,"^[^.].*",REG_EXTENDED,3);
00714   } else {
00715     string value=string_util::trim(arg.substr(arg.find("=")+1));
00716     string key=string_util::trim(arg.substr(0,arg.find("=")));
00717     plist::ObjectBase* ob=sim::config.resolveEntry(key);
00718     if(ob==NULL) {
00719       cout << "'" << key << "' is unknown" << endl;
00720       return false;
00721     }
00722     if(plist::PrimitiveBase* pbp=dynamic_cast<plist::PrimitiveBase*>(ob)) {
00723       try {
00724         pbp->set(value);
00725         return true;
00726       } catch(const XMLLoadSave::bad_format& bf) {
00727         cout << "'" << value << "' is a bad value for '" << key << "'" << endl;
00728         cout << bf.what() << endl;
00729       } catch(const std::exception& e) {
00730         cout << "An exception occured: " << e.what() << endl;
00731       }
00732     } else {
00733       cout << "Cannot assign to a dictionary" << endl;
00734       return false;
00735     }
00736   }
00737   return false; //exception occurred
00738 }
00739 void Simulator::cmdRun(const std::vector<std::string>& args, bool isRelative) {
00740   if(args.size()<=1) {
00741     cout << "runfor/runto requires an argument" << endl;
00742     return;
00743   }
00744   if(isRelative)
00745     globals->setAutoPauseTime(get_time()+atoi(args[1].c_str()));
00746   else
00747     globals->setAutoPauseTime(atoi(args[1].c_str()));
00748   if(globals->timeScale==0)
00749     globals->timeScale=runSpeed;
00750 }
00751 void Simulator::cmdRun(const std::vector<std::string>& args) {
00752   if(args.size()<=1) {
00753     if(globals->timeScale!=0) {
00754       cout << "Already running" << endl;
00755       return;
00756     }
00757     globals->timeScale=runSpeed;
00758   } else {
00759     float speed=atof(args[1].c_str());
00760     if(speed!=0)
00761       runSpeed=speed;
00762     globals->timeScale=speed;
00763   }
00764 }
00765 void Simulator::cmdPause(const std::vector<std::string>& args) {
00766   if(globals->timeScale==0) {
00767     if(find(args.begin(),args.end(),"quiet")==args.end())
00768       cout << "Already paused" << endl;
00769     return;
00770   }
00771   runSpeed=globals->timeScale;
00772   globals->timeScale=0;
00773 }
00774 void Simulator::cmdHelp(const std::vector<std::string>& args) {
00775   map<string,string> syntax;
00776   syntax["load"]="[file]";
00777   syntax["save"]="[file]";
00778   syntax["runlevel"]="[";
00779   for(unsigned int i=0; i<SharedGlobals::NUM_RUNLEVELS; i++) {
00780     stringstream ss;
00781     ss << i << "|" << SharedGlobals::runlevel_names[i];
00782     if(i!=SharedGlobals::NUM_RUNLEVELS-1)
00783       ss << " | ";
00784     syntax["runlevel"]+=ss.str();
00785   }
00786   syntax["runlevel"]+="]";
00787   syntax["get_time"]="";
00788   syntax["set"]="[var=value]";
00789   syntax["runto"]="time";
00790   syntax["runfor"]="time";
00791   syntax["run"]="[speed]";
00792   syntax["pause"]="";
00793   syntax["step"]="[camera|sensor|timer|motion]";
00794   syntax["status"]="[Main|Motion|SoundPlay|Simulator|all]*";
00795   syntax["advance"]=syntax["freeze"]=syntax["unfreeze"]="[camera|sensors|all]*";
00796   syntax["reset"]="[camera|sensors|all]";
00797   syntax["new"]="type [name]";
00798   syntax["delete"]="name";
00799   
00800   map<string,string> help;
00801   
00802   help["load"]="Load HAL configuration from file; if file unspecified, uses last specified file ('hal-$MODEL.plist' by default).\n"
00803     "Note that these files are human-readable XML (with comments!), and you can remove values to specify only a subset of settings.";
00804   
00805   help["save"]="Save HAL configuration to file; if file unspecified, uses last specified file ('hal-$MODEL.plist' by default).\n"
00806     "Note that these files are human-readable XML (with comments!), and you can remove values to specify only a subset of settings.";
00807   
00808   help["runlevel"]="You can specify a runlevel to move to, or if unspecified, the next one.\n"
00809     "You can only move forward runlevels, not backward.  Usually you'll only need RUNNING, "
00810     "unless you are debugging startup/shutdown code or the Tekkotsu itself.";
00811   
00812   help["get_time"]="Displays the simulator time.";
00813   
00814   help["set"]="Sets HAL configuration variables.  Without any arguments, displays all available variables and their current values.\n"
00815     "Type 'help set <variable>' to get more information about a particular variable.";
00816   
00817   help["runto"]="Triggers 'run' until the simulator time reaches the specified value and then pauses.";
00818   
00819   help["runfor"]="Triggers 'run' until the simulator time has moved by the specified number of milliseconds, then pauses.";
00820   
00821   help["run"]="Resets speed to last non-zero value (i.e. value prior to last 'pause'), can override by passing a new value as argument.  Can be abbreviated 'r'.";
00822   
00823   help["pause"]="Equivalent to 'set Speed=0'.  Can be abbreviated 'p'.  Stops the flow of time within the simulator.";
00824   
00825   help["step"]="Runs at \"full\" speed until the next indicated time frame, or the next available frame if no type is specified.\n"
00826     "See 'status' for available frames.";
00827   
00828   help["status"]="Displays a status report regarding current time, upcoming keyframes, and semaphore usage.  Specify one or more processes to get more in-depth, per-process status reports.";
00829   
00830   help["advance"]="Sends the next frame for the specified queue(s) in their listed order (can be listed more than once).\n"
00831     "Disregards timestamp information, and doesn't advance time, unlike 'step' command.  No arguments and \"all\" is the same as \"sensors camera\".";
00832   
00833   help["freeze"]="Equivalent to 'set queue.Frozen=true'.\n"
00834     "Stops sending frames from the specified queue(s), but still allows time to move (unlike 'pause').  No arguments is the same as \"all\".  See 'advance' and 'unfreeze'.";
00835   
00836   help["unfreeze"]="Equivalent to 'set queue.Frozen=false'.\n"
00837     "Begin sending frames from the specified queue(s) again.  Timestamps for the file listing are offset by the time spent frozen minus frames advanced so the queue(s) will continue from their current position.  No arguments is the same as \"all\".";
00838   
00839   help["reset"]="Moves the specified data queue(s) back to the first entry in their list.";
00840   
00841   help["new"]="Creates a new driver or communication port instance.\n  Driver types are:";
00842   set<string> driverNames;
00843   DeviceDriver::getRegistry().getTypeNames(driverNames);
00844   for(set<string>::iterator it=driverNames.begin(); it!=driverNames.end(); ++it)
00845     help["new"]+=" "+*it;
00846   set<string> commNames;
00847   CommPort::getRegistry().getTypeNames(commNames);
00848   help["new"]+="\n  Communication ports types are:";
00849   for(set<string>::iterator it=commNames.begin(); it!=commNames.end(); ++it)
00850     help["new"]+=" "+*it;
00851   
00852   help["delete"]="Remove an entry from the CommPort or Drivers list";
00853     
00854   if(args.size()==1) {
00855     cout << "Available commands: " << endl;
00856     for(map<string,string>::const_iterator it=help.begin(); it!=help.end(); ++it) {
00857       cout << '\t' << it->first << " " << syntax[it->first] << endl;
00858     }
00859     cout << "type 'help <command>' for more information" << endl;
00860   } else {
00861     if(help.find(args[1])==help.end()) {
00862       cout << "The command '"<< args[1] << "' was not found" << endl;
00863       return;
00864     }
00865     if(args.size()==2) {
00866       cout << args[1] << " " << syntax[args[1]] << endl;
00867       cout << help[args[1]] << endl;
00868     } else {
00869       if(args[1]=="set") {
00870         plist::ObjectBase* ob=sim::config.resolveEntry(args[2]);
00871         if(ob==NULL) {
00872           cout << "'" << args[2] << "' is unknown" << endl;
00873           return;
00874         }
00875         size_t n=args[2].rfind('.');
00876         if(n==string::npos)
00877           cout << sim::config.getComment(args[2]) << endl;
00878         else {
00879           ob=sim::config.resolveEntry(args[2].substr(0,n));
00880           if(const plist::Dictionary * dict=dynamic_cast<const plist::Dictionary*>(ob))
00881             cout << dict->getComment(args[2].substr(n+1)) << endl;
00882           else
00883             cout << "'" << args[2].substr(0,n) << "' is not a dictionary" << endl;
00884         }
00885       } else {
00886         cout << args[1] << " " << syntax[args[1]] << endl;
00887         cout << help[args[1]] << endl;
00888       }
00889     }
00890   }
00891 }
00892 void Simulator::cmdStep(const std::vector<std::string>& args) {
00893   if(args.size()<=1) {
00894     if(globals->timeScale!=0)
00895       globals->timeScale=0;
00896     step=STEP_NONE;
00897     incrementTime();
00898     return;
00899   }
00900   if(args.size()>2) {
00901     cout << args[0] << " takes 0 or 1 arguments; " << args.size()-1 << " supplied" << endl;
00902     return;
00903   }
00904   if(args[1]=="camera")
00905     step=STEP_CAMERA;
00906   else if(args[1]=="sensor" || args[1]=="sensors")
00907     step=STEP_SENSOR;
00908   else if(args[1]=="timer")
00909     step=STEP_TIMER;
00910   else if(args[1]=="motion")
00911     step=STEP_MOTION;
00912   else {
00913     cout << args[1] << " is not a valid argument for 'step'.  Type 'help step'." << endl;
00914     return;
00915   }
00916   if(step==STEP_CAMERA && vision.frozen && !vision.heartbeat) {
00917     cout << "Camera queue is frozen and has no heartbeat, cannot step (use 'advance' instead)" << endl;
00918     step=STEP_NONE;
00919   } else if(step==STEP_SENSOR && sensors.frozen && !sensors.heartbeat) {
00920     cout << "Sensor queue is frozen and has no heartbeat, cannot step (use 'advance' instead)" << endl;
00921     step=STEP_NONE;
00922   } else {
00923     unsigned int steps=getNextFrame();
00924     if((1<<step) & steps) { // the desired step is the next step -- just increment
00925       if(globals->timeScale!=0)
00926         globals->timeScale=0;
00927       step=STEP_NONE;
00928       incrementTime();
00929     } else if(globals->timeScale!=-1)
00930       globals->timeScale=-1;
00931   }
00932 }
00933 void Simulator::cmdStatus(const std::vector<std::string>& args) {
00934   cout << "Speed is " << static_cast<float>(globals->timeScale);
00935   if(globals->timeScale<0)
00936     cout << " (full speed mode: avg speed=" << ((globals->simulatorTime-fullspeedSimStart)/fullspeedWallStart.Age().Value()/1000) << "x, "
00937       << " current speed=" << (avgSimTime/avgWallTime/1000) << "x)";
00938   cout << endl;
00939   cout << "Current time is " << get_time() << endl;
00940   unsigned int vis=vision.nextTimestamp();
00941   unsigned int sen=sensors.nextTimestamp();
00942   unsigned int tim=globals->getNextTimer();
00943   unsigned int mot=globals->getNextMotion();
00944   cout << "Next camera: ";
00945   if(vis==-1U) cout << "(none)"; else {
00946     if(vision.frozen)
00947       cout << "Frozen@";
00948     cout << vision.getDataSource()->nextName();
00949     if(!vision.frozen || vision.heartbeat) {
00950       if(vision.frozen && vision.heartbeat && vision.getDataSource()->nextName()!="heartbeat")
00951         cout << " heartbeat";
00952       cout << " scheduled at " << vis;
00953     }
00954   }
00955   cout << endl;
00956   cout << "Next sensor: ";
00957   if(sen==-1U) cout << "(none)"; else {
00958     if(sensors.frozen)
00959       cout << "Frozen@";
00960     cout << sensors.getDataSource()->nextName();
00961     if(!sensors.frozen || sensors.heartbeat) {
00962       if(sensors.frozen && sensors.heartbeat && sensors.getDataSource()->nextName()!="heartbeat")
00963         cout << " heartbeat";
00964       cout << " scheduled at " << sen;
00965     }
00966   }
00967   cout << endl;
00968   cout << "Next timer: ";
00969   if(tim==-1U) cout << "(none)"; else cout << tim;
00970   cout << endl;
00971   cout << "Next motion: ";
00972   if(mot==-1U) cout << "(none)"; else cout << mot;
00973   cout << endl;
00974   unsigned int semUsed=MessageQueueBase::getSemaphoreManager()->used();
00975   unsigned int semMax=semUsed+MessageQueueBase::getSemaphoreManager()->available();
00976   cout << "Semaphores used: " << semUsed << "/" << semMax << " (" << ((semUsed*10000+5)/semMax/10)/10.f << "%)" << endl;
00977   cout << endl;
00978   if(args.size()>1) {
00979     SemaphoreManager::semid_t sem=statusRequest->addReadStatusListener();
00980     for(unsigned int i=1; i<args.size(); ++i) {
00981       RCRegion * region=new RCRegion(args[i].size()+1);
00982       strncpy(region->Base(),args[i].c_str(),args[i].size()+1);
00983       statusRequest->sendMessage(region);
00984       region->RemoveReference();
00985     }
00986     //wait until they're done to put the prompt back up
00987     if(sem!=statusRequest->getSemaphoreManager()->invalid()) {
00988       statusRequest->getSemaphoreManager()->lower(sem,args.size()-1);
00989       statusRequest->removeReadStatusListener(sem);
00990     }
00991     //check to see if we're included:
00992     //haha, now I remember why I don't use functional programming
00993     /*if(find_if(args.begin()+1,args.end(),not1(__gnu_cxx::compose1(bind2nd(ptr_fun(strcasecmp),getName().c_str()),mem_fun_ref(&string::c_str))))!=args.end())
00994       statusReport(cout);*/
00995     for(unsigned int i=1; i<args.size(); ++i) {
00996       if(strcasecmp(args[i].c_str(),getName())==0 || strcasecmp(args[i].c_str(),"all")==0) {
00997         statusReport(cout);
00998         cout << endl;
00999       }
01000     }
01001   }
01002 }
01003 void Simulator::cmdAdvance(const std::vector<std::string>& args) {
01004   if(curLevel!=SharedGlobals::RUNNING) {
01005     cout << args[0] << " can only be used in the RUNNING runlevel" << endl;
01006     return;
01007   }
01008   std::string senstr="sensors";
01009   std::string camstr="camera";
01010   std::vector<std::string> queuenames;
01011   bool isAll=false;
01012   if(args.size()<=1) { // no arguments supplied, advance all
01013     queuenames.push_back(senstr);
01014     queuenames.push_back(camstr);
01015     isAll=true;
01016   }
01017   for(unsigned int i=1; i<args.size(); ++i) {
01018     if(args[i]==camstr)
01019       queuenames.push_back(camstr);
01020     else if(args[i]==senstr)
01021       queuenames.push_back(senstr);
01022     else if(args[i]=="all") {
01023       queuenames.push_back(senstr);
01024       queuenames.push_back(camstr);
01025       isAll=true;
01026     } else {
01027       cout << "invalid argument for advance command: " << args[i] << endl;
01028       return;
01029     }
01030   }
01031   for(std::vector<std::string>::iterator it=queuenames.begin(); it!=queuenames.end(); ++it) {
01032     LoadDataThread * lft=NULL;
01033     MessageQueueBase* q=NULL;
01034     if(*it==camstr) {
01035       lft=&vision;
01036       q=&(*cameraQueue);
01037     } else if(*it==senstr) {
01038       lft=&sensors;
01039       q=&(*sensorQueue);
01040     } else {
01041       cout << "Simulator: internal error, invalid queue " << *it << endl;
01042       return;
01043     }
01044     SemaphoreManager::semid_t sem=q->addReadStatusListener(); //register read status listener before sending!
01045     bool sent=lft->advanceFrame(true); // send frame
01046     if(!sent) { // no data in queue
01047       sent=lft->heartbeat; // but there may have been a heartbeat?
01048       if(!isAll) {
01049         // only report empty queue if the queue was explicitly specified
01050         cout << "No data in " << *it << " queue";
01051         if(lft->heartbeat)
01052           cout << ", sent heartbeat";
01053         cout << endl;
01054       }
01055     }
01056     if(sent)
01057       q->getSemaphoreManager()->lower(sem,true); //block until we know message was read
01058     q->removeReadStatusListener(sem);
01059   }
01060 }
01061 void Simulator::cmdFreeze(bool v, const std::vector<std::string>& args) {
01062   std::string senstr="sensors";
01063   std::string camstr="camera";
01064   std::set<std::string> queuenames;
01065   if(args.size()<=1) {
01066     queuenames.insert(camstr);
01067     queuenames.insert(senstr);
01068   }
01069   for(unsigned int i=1; i<args.size(); ++i) {
01070     if(args[i]==camstr)
01071       queuenames.insert(camstr);
01072     else if(args[i]==senstr)
01073       queuenames.insert(senstr);
01074     else if(args[i]=="all") {
01075       queuenames.insert(camstr);
01076       queuenames.insert(senstr);
01077     } else {
01078       cout << "invalid argument for freeze/unfreeze command: " << args[i] << endl;
01079       return;
01080     }
01081   }
01082   for(std::set<std::string>::iterator it=queuenames.begin(); it!=queuenames.end(); ++it) {
01083     if(*it==camstr)
01084       vision.frozen=v;
01085     else if(*it==senstr)
01086       sensors.frozen=v;
01087     else {
01088       cout << "Simulator: internal error, invalid queue " << *it << endl;
01089       return;
01090     }
01091   }
01092 }
01093 void Simulator::cmdReset(const std::vector<std::string>& args) {
01094   std::string senstr="sensors";
01095   std::string camstr="camera";
01096   for(unsigned int i=1; i<args.size(); ++i) {
01097     if(args[i]==camstr) {
01098       if(vision.getDataSource()!=NULL)
01099         vision.getDataSource()->reset();
01100     } else if(args[i]==senstr) {
01101       if(sensors.getDataSource()!=NULL)
01102         sensors.getDataSource()->reset();
01103     } else if(args[i]=="all") {
01104       if(vision.getDataSource()!=NULL)
01105         vision.getDataSource()->reset();
01106       if(sensors.getDataSource()!=NULL)
01107         sensors.getDataSource()->reset();
01108     } else
01109       cout << "invalid argument for reset command: " << args[i] << endl;
01110   }
01111 }
01112 void Simulator::cmdNew(const std::vector<std::string>& args) {
01113   set<string> driverNames;
01114   DeviceDriver::getRegistry().getTypeNames(driverNames);
01115   set<string> commNames;
01116   CommPort::getRegistry().getTypeNames(commNames);
01117   if(args.size()<2) {
01118     cerr << "Must specify type to instantiate:\n";
01119     cerr << "  Communication Ports:";
01120     for(set<string>::iterator it=commNames.begin(); it!=commNames.end(); ++it)
01121       cerr << ' ' << *it;
01122     cerr << endl;
01123     cerr << "  Device Drivers:";
01124     for(set<string>::iterator it=driverNames.begin(); it!=driverNames.end(); ++it)
01125       cerr << ' ' << *it;
01126     cerr << endl;
01127     return;
01128   }
01129   std::string type = args[1];
01130   std::string name = (args.size()>2) ? args[2] : args[1];
01131   try {
01132     if(driverNames.find(type)!=driverNames.end()) {
01133       DeviceDriver * d = DeviceDriver::getRegistry().create(type,name);
01134       if(d==NULL) {
01135         cerr << "Error instantiating " << type << " (instance with same name already exists?)" << endl;
01136         return;
01137       }
01138     } else if(commNames.find(type)!=commNames.end()) {
01139       CommPort * c = CommPort::getRegistry().create(type,name);
01140       if(c==NULL) {
01141         cerr << "Error instantiating " << type << " (instance with same name already exists?)" << endl;
01142         return;
01143       }
01144     } else {
01145       cerr << "'" << type << "' is not a valid type for instantiation.  Please choose one of:\n";
01146       cerr << "  Communication Ports:";
01147       for(set<string>::iterator it=commNames.begin(); it!=commNames.end(); ++it)
01148         cerr << ' ' << *it;
01149       cerr << endl;
01150       cerr << "  Device Drivers:";
01151       for(set<string>::iterator it=driverNames.begin(); it!=driverNames.end(); ++it)
01152         cerr << ' ' << *it;
01153       cerr << endl;
01154     }
01155   } catch(const std::exception& e) {
01156     cout << "An exception occured during construction of "<<type<<": " << e.what() << endl;
01157   } catch(...) {
01158     cout << "An exception occured during construction of "<<type << endl;
01159   }
01160 }
01161 
01162 void Simulator::cmdDelete(const std::vector<std::string>& args) {
01163   if(args.size()<2) {
01164     cerr << "Must specify instance to delete:\n";
01165     cerr << "  Communication Ports:";
01166     for(CommPort::registry_t::const_iterator it=CommPort::getRegistry().begin(); it!=CommPort::getRegistry().end(); ++it)
01167       cerr << ' ' << it->first;
01168     cerr << endl;
01169     cerr << "  Device Drivers:";
01170     for(DeviceDriver::registry_t::const_iterator it=DeviceDriver::getRegistry().begin(); it!=DeviceDriver::getRegistry().end(); ++it)
01171       cerr << ' ' << it->first;
01172     cerr << endl;
01173     return;
01174   }
01175   if(DeviceDriver::getRegistry().getInstance(args[1])!=NULL) {
01176     if(!DeviceDriver::getRegistry().destroy(args[1]))
01177       cerr << "Could not delete driver named '" << args[1] << "'" << endl;
01178   } else if(CommPort::getRegistry().getInstance(args[1])!=NULL) {
01179     if(!CommPort::getRegistry().destroy(args[1]))
01180       cerr << "Could not delete comm port named '" << args[1] << "'" << endl;
01181   } else {
01182     cerr << "Could not find driver to delete named '" << args[1] << "'" << endl;
01183     return;
01184   }
01185 }
01186 
01187 
01188 /*! @file
01189  * @brief 
01190  * @author ejt (Creator)
01191  *
01192  * $Author: ejt $
01193  * $Name: tekkotsu-4_0 $
01194  * $Revision: 1.13 $
01195  * $State: Exp $
01196  * $Date: 2007/11/19 21:00:41 $
01197  */
01198 

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