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
00062
00063
00064 sim::config.setSavePolicy(plist::Collection::UNION);
00065
00066 sim::config.writeParseTree();
00067
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
00073 sim::config.setUnusedWarning(true);
00074
00075 sim::config.readParseTree();
00076
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
00086 vector<vector<string> > delayed;
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);
00096 }
00097 }
00098 for(unsigned int i=0; i<delayed.size(); i++) {
00099
00100
00101 if(!cmdSet(delayed[i]))
00102 cerr << "Occurred while processing " << delayed[i][1] << endl;
00103 }
00104
00105
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
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();
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 ) {
00265 MarkScope l(simLock);
00266 if(globals->timeScale<0) {
00267
00268 if(&mq==&(*cameraQueue)) {
00269
00270 waitingSteps&=~(1<<STEP_CAMERA);
00271 } else if(&mq==&(*sensorQueue)) {
00272
00273 waitingSteps&=~(1<<STEP_SENSOR);
00274 } else if(&mq==&(*timerWakeup)) {
00275
00276 waitingSteps&=~(1<<STEP_TIMER);
00277 } else if(&mq==&(*motionWakeup)) {
00278
00279 waitingSteps&=~(1<<STEP_MOTION);
00280 } else if(&mq==&(*events)) {
00281
00282
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
00287
00288 if(waitingSteps==0 && events->getMessagesUnread()==0)
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
00305
00306 for(std::set<MotionHook*>::iterator it=theSim->motionHooks.begin(); it!=theSim->motionHooks.end(); ++it)
00307 (*it)->motionStarting();
00308 }
00309 void Simulator::setMotionStopping() {
00310
00311
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;
00357
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
00403 }
00404 if(sensors.isStarted()) {
00405 sensors.stop();
00406
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
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
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
00481 globals->timeScale=0;
00482 step=STEP_NONE;
00483 globals->setAutoPauseTime(-1U);
00484 }
00485 }
00486
00487 unsigned int Simulator::getNextFrame() {
00488 frameTimes.clear();
00489
00490
00491
00492
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
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);
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
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
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>& ) {
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;
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) {
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
00987 if(sem!=statusRequest->getSemaphoreManager()->invalid()) {
00988 statusRequest->getSemaphoreManager()->lower(sem,args.size()-1);
00989 statusRequest->removeReadStatusListener(sem);
00990 }
00991
00992
00993
00994
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) {
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();
01045 bool sent=lft->advanceFrame(true);
01046 if(!sent) {
01047 sent=lft->heartbeat;
01048 if(!isAll) {
01049
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);
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
01189
01190
01191
01192
01193
01194
01195
01196
01197
01198