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