00001 #include "sim.h"
00002 #include "Main.h"
00003 #include "Motion.h"
00004 #include "SoundPlay.h"
00005 #include "Simulator.h"
00006 #include "SharedGlobals.h"
00007 #include "SimConfig.h"
00008 #include "local/DataSource.h"
00009 #include "IPC/SharedObject.h"
00010 #include "IPC/RegionRegistry.h"
00011 #include "Shared/Config.h"
00012 #include "Shared/string_util.h"
00013 #include "Shared/StackTrace.h"
00014 #include "Shared/zignor.h"
00015 #include "Shared/RobotInfo.h"
00016 #include "IPC/SemaphoreManager.h"
00017 #include "Events/EventRouter.h"
00018 #include <iostream>
00019 #include <sstream>
00020 #include <iomanip>
00021 #include <sys/ipc.h>
00022 #include <signal.h>
00023 #include <unistd.h>
00024 #include <sys/wait.h>
00025 #include <sys/stat.h>
00026 #include <errno.h>
00027 #include <pwd.h>
00028
00029 #ifndef DISABLE_READLINE
00030 # include <readline/readline.h>
00031 #endif
00032
00033 #include "Events/EventBase.h"
00034 #include "Events/LocomotionEvent.h"
00035 #include "Events/TextMsgEvent.h"
00036 #include "Events/VisionObjectEvent.h"
00037
00038 #ifndef TEKKOTSU_SHM_STYLE
00039 # error TEKKOTSU_SHM_STYLE unset!
00040 #endif
00041
00042 using namespace std;
00043
00044
00045
00046 template<> std::map<std::string,SharedGlobals::runlevel_t> plist::NamedEnumeration<SharedGlobals::runlevel_t>::namesToVals = std::map<std::string,SharedGlobals::runlevel_t>();
00047 template<> std::map<SharedGlobals::runlevel_t,std::string> plist::NamedEnumeration<SharedGlobals::runlevel_t>::valsToNames = std::map<SharedGlobals::runlevel_t,std::string>();
00048
00049 const char * const sim::usage=
00050 "[-h|--help] [-c|--config config-file] [cmd1 cmd2 ...]\n"
00051 "Commands passed as arguments are commonly of the form var=val, but can\n"
00052 "also be any valid simulator command, such as 'freeze'. If you wish to\n"
00053 "pass a multi-word command, encase the command in quotes.";
00054
00055
00056 bool sim::original=true;
00057
00058 SimConfig sim::config;
00059 vector<string> sim::cmdlineArgs;
00060 pid_t sim::child=static_cast<pid_t>(-1);
00061 vector<Thread*> sim::primaries;
00062 sim::ConfigErrorCheck sim::cfgCheck;
00063
00064
00065
00066 int main(int argc, const char* argv[]) {
00067
00068
00069
00070 ProcessID::setID(ProcessID::SimulatorProcess);
00071
00072
00073 Thread::initMainThread();
00074
00075 #if TEKKOTSU_SHM_STYLE==POSIX_SHM
00076 #ifndef USE_UNBACKED_SHM
00077
00078 struct passwd * pw=getpwuid(getuid());
00079 if(pw!=NULL) {
00080 if(RCRegion::shmRoot[RCRegion::shmRoot.size()-1]=='/') {
00081 RCRegion::shmRoot[RCRegion::shmRoot.size()-1]='-';
00082 RCRegion::shmRoot+=pw->pw_name;
00083 RCRegion::shmRoot+='/';
00084 } else {
00085 RCRegion::shmRoot+=pw->pw_name;
00086 }
00087 }
00088 #endif
00089 #elif TEKKOTSU_SHM_STYLE==NO_SHM
00090 sim::cfgCheck.holdMultiprocess();
00091 #endif
00092
00093 {
00094 sim s;
00095 if(!s.processCommandLine(argc,argv))
00096 return 2;
00097
00098 if(!s.run())
00099 return 1;
00100 return 0;
00101 }
00102 }
00103
00104 sim::sim() :
00105 mutexSemMgr(),
00106 glob(),
00107 subj()
00108 {
00109
00110 MutexLockBase::usleep_granularity=measure_usleep_cost();
00111
00112 project_get_time::get_time_callback=&sim_get_time;
00113 project_get_time::get_timeScale_callback=&sim_getTimeScale;
00114
00115 ProjectInterface::sendCommand=Simulator::sendCommand;
00116
00117
00118 signal(SIGHUP, handle_signal);
00119 signal(SIGINT, handle_signal);
00120 signal(SIGQUIT, handle_signal);
00121 signal(SIGILL, handle_signal);
00122 signal(SIGABRT, handle_signal);
00123 signal(SIGFPE, handle_signal);
00124 signal(SIGBUS, handle_signal);
00125 signal(SIGSEGV, handle_signal);
00126 signal(SIGSYS, handle_signal);
00127 signal(SIGPIPE, handle_signal);
00128 signal(SIGTERM, handle_signal);
00129 atexit(handle_exit);
00130
00131 #if TEKKOTSU_SHM_STYLE!=NO_SHM
00132
00133 MutexLockBase::setSemaphoreManager(&(*mutexSemMgr));
00134 #endif
00135
00136 MessageQueueBase::setSemaphoreManager(&(*mutexSemMgr));
00137
00138
00139 globals = &(*glob);
00140 sim::config.addEntry("WaitForSensors",globals->waitForSensors,"If true, wait for initial sensor readings before triggering the startup behavior or starting the motion polling thread. On some platforms, sensed output values can be used to initialize output positions. On others, you may be unable to get any feedback, or can only expect feedback if the robot was left running and the executable is reconnecting.");
00141 sim::config.addEntry("Speed",globals->timeScale,"The speed at which to run the simulation, as a multiple of \"real-time\".\nFor example, '1' is normal, '0.5' is half-speed, '0' is paused.\nAny negative value requests non-realtime mode, where the clock is moved as fast as processing (or manual control) allows.");
00142 globals->simulatorTime=sim::config.initSimTime;
00143 sim::config.addEntry("Motion",globals->motion,"Parameters for motion simulation");
00144 DataSource::setOutputTracker(globals->motion.providedOutputs);
00145
00146
00147 ipc_setup = &(*subj);
00148
00149
00150 ::config = new Config();
00151 ::config->setFileSystemRoot("ms");
00152 if(::config->loadFile("config/tekkotsu.xml")==0) {
00153 if(::config->loadFile("config/tekkotsu.cfg")==0)
00154 std::cerr << std::endl << " *** ERROR: Could not load configuration file config/tekkotsu.xml *** " << std::endl << std::endl;
00155 else
00156 std::cerr << "Successfully imported settings from old-format tekkotsu.cfg" << std::endl;
00157 }
00158 if(::config->loadFile("config/sim_ovrd.xml")==0)
00159 if(::config->loadFile("config/sim_ovrd.cfg")!=0)
00160 std::cerr << "Successfully imported settings from old-format simulator.cfg" << std::endl;
00161 ::erouter = new EventRouter;
00162
00163
00164 if(::config->main.seed_rng) {
00165 struct timeval tp;
00166 gettimeofday(&tp,NULL);
00167 tp.tv_sec+=tp.tv_usec;
00168 RanNormalSetSeedZig32((int*)&tp.tv_sec,tp.tv_usec);
00169
00170 double t=TimeET().Value();
00171 unsigned int * tm=reinterpret_cast<unsigned int*>(&t);
00172 unsigned int seed=tm[0]+tm[1];
00173 cout << "RNG seed=" << seed << ", zignor seeds: " << tp.tv_sec << ',' << tp.tv_usec << endl;;
00174 srand(seed);
00175 } else {
00176 int t=12345;
00177 int u=54321;
00178 RanNormalSetSeedZig32(&t,u);
00179 }
00180 }
00181
00182 bool sim::processCommandLine(int argc, const char* argv[]) {
00183 int i=0;
00184
00185
00186 string halconfigfile;
00187 halconfigfile.append("hal-").append(RobotName).append(".plist");
00188 struct stat sb;
00189 if(stat(halconfigfile.c_str(),&sb)==0) {
00190 if(!sim::config.loadFile(halconfigfile.c_str())) {
00191 cerr << "Error loading default configuration file '" << halconfigfile << "', may be malformed." << endl;
00192 return false;
00193 }
00194 } else {
00195
00196 string defaultfile;
00197 defaultfile.assign("defaults/hal-common.plist");
00198 if(stat(defaultfile.c_str(),&sb)==0) {
00199 if(!sim::config.loadFile(defaultfile.c_str())) {
00200 cerr << "Error loading defaults file '" << defaultfile << "', may be malformed." << endl;
00201 return false;
00202 }
00203 }
00204 defaultfile.assign("defaults/hal-").append(RobotName).append(".plist");
00205 if(stat(defaultfile.c_str(),&sb)==0) {
00206 if(!sim::config.loadFile(defaultfile.c_str())) {
00207 cerr << "Error loading defaults file '" << defaultfile << "', may be malformed." << endl;
00208 return false;
00209 }
00210 }
00211 sim::config.setLastFile(halconfigfile);
00212 }
00213
00214
00215
00216
00217
00218
00219
00220
00221 config.cmdPrompt="HAL:";
00222 config.cmdPrompt.append(RobotName).append("> ");
00223
00224
00225 for(i++; i<argc; i++) {
00226 if(!strcmp(argv[i],"-h") || !strcmp(argv[i],"--help")) {
00227 cerr << argv[0] << ": " << usage << endl;
00228 return false;
00229 } else if(!strcmp(argv[i],"-c") || !strcmp(argv[i],"--config")) {
00230 if(!sim::config.loadFile(argv[++i]))
00231 return false;
00232 } else if(strchr(argv[i],'=')!=NULL) {
00233 string value=string_util::trim(strchr(argv[i],'=')+1);
00234 string key=string_util::trim(string(argv[i],strchr(argv[i],'=')-argv[i]));
00235 plist::ObjectBase* ob=sim::config.resolveEntry(key);
00236 if(ob==NULL)
00237 cmdlineArgs.push_back(argv[i]);
00238 else if(plist::PrimitiveBase* pbp=dynamic_cast<plist::PrimitiveBase*>(ob)) {
00239 try {
00240 pbp->set(value);
00241 } catch(const XMLLoadSave::bad_format& bf) {
00242 cout << "'" << value << "' is a bad value for '" << key << "'" << endl;
00243 cout << bf.what() << endl;
00244 return false;
00245 } catch(const std::exception& e) {
00246 cout << "An exception occured: " << e.what() << endl;
00247 return false;
00248 }
00249 } else {
00250 cout << "Cannot assign to a dictionary ("<<key<<")" << endl;
00251 return false;
00252 }
00253 } else {
00254 cmdlineArgs.push_back(argv[i]);
00255 }
00256 }
00257
00258 return true;
00259 }
00260
00261 bool sim::run() {
00262
00263 cfgCheck.holdMultiprocess();
00264 RCRegion::setMultiprocess(config.multiprocess);
00265 if(!config.multiprocess) {
00266 ProcessID::setIDHooks(getProcessID,setProcessID);
00267
00268
00269 if(wireless==NULL) {
00270 wireless = new Wireless();
00271 sout=wireless->socket(Socket::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*12);
00272 serr=wireless->socket(Socket::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*4);
00273 wireless->setDaemon(sout);
00274 wireless->setDaemon(serr);
00275 serr->setFlushType(Socket::FLUSH_BLOCKING);
00276 sout->setTextForward();
00277 serr->setForward(sout);
00278 }
00279
00280
00281 if(kine==NULL)
00282 kine=new Kinematics();
00283 }
00284
00285
00286
00287 globals->level_count[SharedGlobals::CREATED]++;
00288
00289 cout << "Spawning processes..." << endl;
00290 cout.setf(ios::left);
00291 cout << " Initializing runlevel " << setw(12) << SharedGlobals::runlevel_names[SharedGlobals::CONSTRUCTING] << endl;
00292 cout.unsetf(ios::left);
00293 if(fork_process<Main>()) ;
00294 else if(fork_process<Motion>()) ;
00295 else if(fork_process<SoundPlay>()) ;
00296 else if(config.multiprocess)
00297 manage_process<Simulator>();
00298 else {
00299 fork_process<Simulator>();
00300 globals->level_count[SharedGlobals::CREATED]--;
00301 while(primaries.size()>0) {
00302 primaries.back()->join();
00303 delete primaries.back();
00304 primaries.pop_back();
00305 }
00306 }
00307
00308
00309 globals->level_count[SharedGlobals::DESTRUCTED]++;
00310
00311 return true;
00312 }
00313
00314 sim::~sim() {
00315 #if TEKKOTSU_SHM_STYLE!=NO_SHM
00316 MutexLockBase::setSemaphoreManager(NULL);
00317 #endif
00318 MessageQueueBase::setSemaphoreManager(NULL);
00319 globals=NULL;
00320 ipc_setup=NULL;
00321
00322 if(child==static_cast<pid_t>(-1))
00323 return;
00324
00325 if(original)
00326 cout << ProcessID::getIDStr() << ": Waiting for children to exit..." << endl;
00327 if(child!=0) {
00328 int status;
00329 int res=waitpid(child,&status,0);
00330 if(res<0 && errno!=ECHILD)
00331 perror("wait");
00332 if(!WIFEXITED(status))
00333 cout << ProcessID::getIDStr() << ": waiting for " << child << "..." << endl;
00334 while(!WIFEXITED(status)) {
00335 sleep(1);
00336 res=waitpid(child,&status,0);
00337 if(res<0 && errno!=ECHILD)
00338 perror("wait");
00339 }
00340 }
00341 }
00342
00343 void sim::wait_runlevel(SharedGlobals::runlevel_t level) {
00344 globals->lock.lock(ProcessID::getID());
00345 globals->level_count[level]++;
00346 if(globals->level_count[level]==1) {
00347 cout.setf(ios::left);
00348 cout << "Collecting for runlevel " << setw(12) << SharedGlobals::runlevel_names[level] << " |=" << flush;
00349 cout.unsetf(ios::left);
00350 }
00351 string nm=Process::getName();
00352 cout << nm << '=' << flush;
00353 if(globals->level_count[level]==globals->level_count[SharedGlobals::CREATED])
00354 cout << "| done" << endl;
00355 globals->lock.unlock();
00356 while(globals->level_count[level]!=globals->level_count[SharedGlobals::CREATED])
00357 usleep(150*1000);
00358 globals->lock.lock(ProcessID::getID());
00359 globals->lock.unlock();
00360 }
00361
00362 template<class T>
00363 void sim::manage_process() {
00364
00365 globals->lock.lock(T::getID());
00366 globals->level_count[SharedGlobals::CONSTRUCTING]++;
00367 cout << setw(35) << T::getClassName() << ": ProcessID::getID()=" << T::getID() << " pid=" << getpid() << endl;
00368
00369 T t;
00370 ASSERT(T::getID()==ProcessID::getID(),"Process ID set incorrectly!");
00371
00372 globals->lock.unlock();
00373 while(globals->level_count[SharedGlobals::CONSTRUCTING]!=globals->level_count[SharedGlobals::CREATED])
00374 usleep(150*1000);
00375 globals->lock.lock(ProcessID::getID());
00376 globals->lock.unlock();
00377
00378
00379 wait_runlevel(SharedGlobals::STARTING);
00380 t.DoStart();
00381 wait_runlevel(SharedGlobals::RUNNING);
00382 t.run();
00383 wait_runlevel(SharedGlobals::STOPPING);
00384 t.DoStop();
00385 wait_runlevel(SharedGlobals::DESTRUCTING);
00386 }
00387
00388 template<class T>
00389 bool sim::fork_process() {
00390 if(config.multiprocess) {
00391 RCRegion::aboutToFork(T::getID());
00392 MutexLockBase::aboutToFork();
00393 }
00394
00395 globals->level_count[SharedGlobals::CREATED]++;
00396 if(config.multiprocess) {
00397 child=fork();
00398 if(child==static_cast<pid_t>(-1)) {
00399 cerr << "Unable to spawn simulator process!" << endl;
00400 exit(1);
00401 }
00402 if(child!=0) {
00403 manage_process<T>();
00404 return true;
00405 }
00406 original=false;
00407 } else {
00408 primaries.push_back(new PrimaryThread<T>());
00409 primaries.back()->start();
00410 }
00411 return false;
00412 }
00413
00414 ProcessID::ProcessID_t sim::getProcessID() {
00415 Thread* th=Thread::getCurrent();
00416 if(th==NULL)
00417 return ProcessID::SimulatorProcess;
00418 return static_cast<ProcessID::ProcessID_t>(reinterpret_cast<size_t>(th->getGroup()));
00419 }
00420 void sim::setProcessID(ProcessID::ProcessID_t id) {
00421 Thread* th=Thread::getCurrent();
00422 ASSERTRET(th!=NULL,"Unable to set process ID for main thread");
00423 th->setGroup(reinterpret_cast<void*>(id));
00424 }
00425
00426 void sim::handle_signal(int sig) {
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436 char * name=NULL;
00437 char defBuf[30];
00438 switch(sig) {
00439 case SIGINT: name="SIGINT"; break;
00440 case SIGQUIT: name="SIGQUIT"; break;
00441 case SIGBUS: name="SIGBUS"; break;
00442 case SIGSEGV: name="SIGSEGV"; break;
00443 case SIGTERM: name="SIGTERM"; break;
00444 case SIGABRT: name="SIGABRT"; break;
00445 case SIGFPE: name="SIGFPE"; break;
00446 case SIGPIPE: name="SIGPIPE"; break;
00447 case SIGHUP: name="SIGHUP"; break;
00448 default:
00449 name=defBuf;
00450 snprintf(name,30,"signal %d",sig);
00451 break;
00452 }
00453 cout << "*** ERROR " << Process::getName() << ": Received " << name << endl;
00454
00455 static bool firstCall=true;
00456 if(!firstCall) {
00457 cerr << "Signal handler was recursively called, may be leaked IPC resources :(" << endl;
00458 cerr << "The 'ipcs' tool can be used to manually free these, if it becomes a problem. " << endl;
00459 cerr << "However, simply re-running will generally reclaim the previous buffers for you." << endl;
00460 exit(EXIT_FAILURE);
00461 return;
00462 }
00463 firstCall=false;
00464
00465 stacktrace::displayCurrentStackTrace(15);
00466
00467 cout << "*** ERROR " << Process::getName() << ": Engaging fault shutdown..." << endl;
00468 if(globals!=NULL && !globals->hadFault()) {
00469 if(!MessageQueueBase::getSemaphoreManager()->hadFault())
00470 globals->lock.lock(ProcessID::getID());
00471 if(globals->level_count[SharedGlobals::CREATED]>0)
00472 globals->level_count[SharedGlobals::CREATED]--;
00473 else
00474 cout << "*** ERROR " << Process::getName() << ": level_count[CREATED] underflow" << endl;
00475 globals->signalShutdown();
00476 if(!MessageQueueBase::getSemaphoreManager()->hadFault())
00477 globals->lock.unlock();
00478 globals->faultShutdown();
00479 }
00480 if(MessageQueueBase::getSemaphoreManager()!=NULL && !MessageQueueBase::getSemaphoreManager()->hadFault()) {
00481 cout << "*** ERROR " << Process::getName() << ": Dereferencing mutex SemaphoreManager" << endl;
00482 MessageQueueBase::setSemaphoreManager(NULL);
00483 MessageQueueBase::getSemaphoreManager()->faultShutdown();
00484 }
00485 cout << "*** ERROR " << Process::getName() << ": Dereferencing shared memory regions" << endl;
00486 RCRegion::faultShutdown();
00487 cout << "*** ERROR " << Process::getName() << ": Exiting..." << endl;
00488 exit(EXIT_FAILURE);
00489 }
00490
00491 void sim::handle_exit() {
00492 static bool firstCall=true;
00493 if(!firstCall) {
00494 cerr << "handle_exit was recursively called" << endl;
00495 return;
00496 }
00497 firstCall=false;
00498 cout << Process::getName() << ": Exiting..." << endl;
00499 #ifndef DISABLE_READLINE
00500 rl_reset_terminal(NULL);
00501 #endif
00502 if(RCRegion::NumberOfAttach()==0) {
00503 if(original)
00504 cout << "Clean shutdown complete. Have a nice day." << endl;
00505 return;
00506 }
00507 cout << "*** ERROR " << Process::getName() << ": Exit with attached memory regions, engaging fault shutdown..." << endl;
00508 if(globals!=NULL && !globals->hadFault()) {
00509 if(!MessageQueueBase::getSemaphoreManager()->hadFault())
00510 globals->lock.lock(ProcessID::getID());
00511 if(globals->level_count[SharedGlobals::CREATED]>0)
00512 globals->level_count[SharedGlobals::CREATED]--;
00513 else
00514 cout << "*** ERROR " << Process::getName() << ": level_count[CREATED] underflow" << endl;
00515 globals->signalShutdown();
00516 if(!MessageQueueBase::getSemaphoreManager()->hadFault())
00517 globals->lock.unlock();
00518 globals->faultShutdown();
00519 } else {
00520 cerr << "*** ERROR " << Process::getName() << ": exit with previous global fault" << endl;
00521 }
00522 if(MessageQueueBase::getSemaphoreManager()!=NULL && !MessageQueueBase::getSemaphoreManager()->hadFault()) {
00523 cout << "*** ERROR " << Process::getName() << ": Dereferencing mutex SemaphoreManager" << endl;
00524 MessageQueueBase::setSemaphoreManager(NULL);
00525 MessageQueueBase::getSemaphoreManager()->faultShutdown();
00526 } else {
00527 cerr << "*** ERROR " << Process::getName() << ": exit with previous mutex fault" << endl;
00528 }
00529 cout << "*** ERROR " << Process::getName() << ": Dereferencing shared memory regions" << endl;
00530 RCRegion::faultShutdown();
00531 cout << "*** ERROR " << Process::getName() << ": Exiting..." << endl;
00532 }
00533
00534 unsigned int sim::measure_usleep_cost() {
00535 usleep(50000);
00536 const unsigned int TRIALS=50;
00537 TimeET mintime(1.0);
00538 for(unsigned int i=0; i<TRIALS; i++) {
00539
00540 TimeET cur;
00541 usleep(1);
00542 TimeET elapsed(cur.Age());
00543 if(elapsed<mintime)
00544 mintime=elapsed;
00545 }
00546 usleep(50000);
00547 TimeET minover(1.0);
00548 for(unsigned int i=0; i<TRIALS; i++) {
00549
00550 TimeET cur;
00551 TimeET elapsed(cur.Age());
00552 if(elapsed<minover)
00553 minover=elapsed;
00554 }
00555 if(mintime<minover) {
00556 mintime=0L;
00557 cout << "usleep granularity couldn't be measured, default to 10ms" << endl;
00558 } else {
00559
00560 mintime-=minover;
00561 cout << "usleep granularity is " << mintime.Value()*1.0e6 << "us";
00562 if(mintime<2L) {
00563 mintime=2L;
00564 cout << ", reset to 2ms";
00565 }
00566 cout << endl;
00567 }
00568 return static_cast<unsigned>(mintime.Value()*1.0e6);
00569 }
00570
00571 sim::ConfigErrorCheck::ConfigErrorCheck() : PrimitiveListener(), holdMPValue(config.multiprocess) {}
00572
00573 sim::ConfigErrorCheck::~ConfigErrorCheck() {
00574 sim::config.multiprocess.removePrimitiveListener(this);
00575 }
00576
00577 void sim::ConfigErrorCheck::plistValueChanged(const plist::PrimitiveBase& pl) {
00578 if(&pl==&sim::config.multiprocess) {
00579 #if TEKKOTSU_SHM_STYLE==NO_SHM
00580 if(sim::config.multiprocess) {
00581 cerr << "ERROR: TEKKOTSU_SHM_STYLE was set to NO_SHM during compile, Muliprocess cannot be set to 'true'" << endl;
00582 sim::config.multiprocess=false;
00583 }
00584 #else
00585 # ifdef DEBUG
00586 cerr << "sim::config.Multiprocess set to " << sim::config.multiprocess << endl;
00587 # endif
00588 if(holdMPValue!=sim::config.multiprocess) {
00589 cerr << "ERROR: Cannot change sim::config.Multiprocess during execution, must set from command line or load from settings file" << endl;
00590 sim::config.multiprocess=holdMPValue;
00591 }
00592 #endif
00593 }
00594 }
00595
00596 void sim::ConfigErrorCheck::holdMultiprocess() {
00597 holdMPValue=sim::config.multiprocess;
00598 sim::config.multiprocess.addPrimitiveListener(this);
00599 }