Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Controller.cc

Go to the documentation of this file.
00001 #include "Controller.h"
00002 #include "Motion/EmergencyStopMC.h"
00003 #include "Motion/MMAccessor.h"
00004 #include "IPC/SharedObject.h"
00005 #include "Shared/WorldState.h"
00006 #include "Shared/get_time.h"
00007 #include "Sound/SoundManager.h"
00008 #include "Events/TextMsgEvent.h"
00009 #include "Shared/RobotInfo.h"
00010 #include "Shared/ERS210Info.h"
00011 #include "Shared/ERS220Info.h"
00012 #include "Shared/ERS2xxInfo.h"
00013 #include "Shared/ERS7Info.h"
00014 #include "Shared/ChiaraInfo.h"
00015 #include "Shared/string_util.h"
00016 #include "Shared/ProjectInterface.h"
00017 #include "Shared/Config.h"
00018 #include "Shared/debuget.h"
00019 
00020 #include "Shared/RobotInfo.h"
00021 #ifdef TGT_HAS_LEDS
00022 #  include "Motion/LedMC.h"
00023 #endif
00024 
00025 #include <sstream>
00026 
00027 Controller * Controller::theOneController=NULL;
00028 
00029 //these are given appropriate values in init once we know which model we're running on
00030 EventBase Controller::nextItem;
00031 EventBase Controller::prevItem;
00032 EventBase Controller::nextItemFast;
00033 EventBase Controller::prevItemFast;
00034 EventBase Controller::selectItem;
00035 EventBase Controller::cancel;
00036 
00037 using namespace string_util;
00038 using namespace std;
00039 
00040 void Controller::doStart() {
00041   BehaviorBase::doStart();
00042   sndman->loadFile(config->controller.select_snd);
00043   sndman->loadFile(config->controller.next_snd);
00044   sndman->loadFile(config->controller.prev_snd);
00045   sndman->loadFile(config->controller.read_snd);
00046   sndman->loadFile(config->controller.cancel_snd);
00047   erouter->addListener(this,EventBase::estopEGID);
00048   // Turn on wireless
00049   gui_comm=wireless->socket(Socket::SOCK_STREAM, 2048, 32000);
00050   wireless->setReceiver(gui_comm->sock, gui_comm_callback);
00051   wireless->setDaemon(gui_comm,true);
00052   wireless->listen(gui_comm->sock, config->controller.gui_port);
00053   theOneController=this;
00054 #ifdef TGT_HAS_LEDS
00055   SharedObject<LedMC> leds;
00056   leds->setWeights(~FaceLEDMask,0);
00057   leds->setWeights(FaceLEDMask,.75f);
00058   display=motman->addPersistentMotion(leds,isControlling?MotionManager::kEmergencyPriority:MotionManager::kIgnoredPriority);
00059 #endif
00060   reset();
00061 }
00062 
00063 void Controller::doStop() {
00064   sndman->releaseFile(config->controller.select_snd);
00065   sndman->releaseFile(config->controller.next_snd);
00066   sndman->releaseFile(config->controller.prev_snd);
00067   sndman->releaseFile(config->controller.read_snd);
00068   sndman->releaseFile(config->controller.cancel_snd);
00069   erouter->removeListener(this);
00070   reset();
00071   motman->removeMotion(display);
00072   display=MotionManager::invalid_MC_ID;
00073   //these two lines help prevent residual display in case that was the only MotionCommand using LEDs
00074 #ifdef TGT_HAS_LEDS
00075   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00076     motman->setOutput(NULL,i,0.f);
00077 #endif
00078   gui_comm->printf("goodbye\n");
00079   wireless->setDaemon(gui_comm,false);
00080   wireless->close(gui_comm);
00081   theOneController=NULL;
00082   BehaviorBase::doStop();
00083 }
00084 
00085 void Controller::doEvent() {
00086   if(event->getTypeID()==EventBase::activateETID) { //estop just turned on
00087     if(!isControlling)
00088       activate();
00089   } else { //estop just turned off
00090     if(isControlling)
00091       deactivate();
00092   }
00093 }
00094 
00095 bool Controller::trapEvent(const EventBase& e) {
00096   if(!chkCmdStack())
00097     return false;
00098   last_time=cur_time;
00099   cur_time=get_time();
00100   //this will prevent inadvertant controller commands when you pick up an ERS-7
00101   if(state->buttons[nextItem.getSourceID()] && state->buttons[prevItem.getSourceID()] && state->buttons[selectItem.getSourceID()])
00102     return true;
00103   
00104   if(nextItem.sameGenSource(e)) {
00105     nextEv_val=e.getMagnitude();
00106     nextEv_dur=e.getDuration();
00107     if(nextEv_val==0 && prevEv_val==0)
00108       alreadyGotBoth=false;
00109     if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666) {
00110       if(alreadyGotBoth)
00111         return true;
00112       else {
00113         alreadyGotBoth=true;
00114         return setNext(cmdstack.top()->doReadStdIn());
00115       }
00116     }
00117     if(e.getTypeID()==nextItem.getTypeID() && e.getDuration()<666)
00118       return setNext(cmdstack.top()->doNextItem());
00119     if(e.getTypeID()==nextItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00120       return setNext(cmdstack.top()->doNextItem());
00121   }
00122   if(prevItem.sameGenSource(e)) {
00123     prevEv_val=e.getMagnitude();
00124     prevEv_dur=e.getDuration();
00125     if(nextEv_val==0 && prevEv_val==0)
00126       alreadyGotBoth=false;
00127     if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666) {
00128       if(alreadyGotBoth)
00129         return true;
00130       else {
00131         alreadyGotBoth=true;
00132         return setNext(cmdstack.top()->doReadStdIn());
00133       }
00134     }
00135     if(e.getTypeID()==prevItem.getTypeID() && e.getDuration()<666)
00136       return setNext(cmdstack.top()->doPrevItem());
00137     if(e.getTypeID()==prevItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00138       return setNext(cmdstack.top()->doPrevItem());
00139   }
00140   if(e.getDuration()>250) {
00141     if(e==selectItem)
00142       return setNext(cmdstack.top()->doSelect());
00143     if(e==cancel)
00144       return setNext(cmdstack.top()->doCancel());
00145   }
00146   return true;
00147 }
00148 
00149 void Controller::reset() {
00150   while(cmdstack.size()>1)
00151     pop();
00152   if(!cmdstack.empty()) {
00153     cmdstack.top()->deactivate();
00154     cmdstack.pop();
00155   }
00156   refresh();
00157 }
00158 
00159 void Controller::refresh() {
00160   if(!chkCmdStack())
00161     return;
00162   cmdstack.top()->refresh();
00163 }
00164 
00165 void Controller::refreshSketchWorld() {
00166   theOneController->gui_comm->printf("refreshsketchworld\n");
00167 }
00168 
00169 void Controller::refreshSketchLocal() {
00170   theOneController->gui_comm->printf("refreshsketchlocal\n");
00171 }
00172 
00173 void Controller::refreshSketchCamera() {
00174   theOneController->gui_comm->printf("refreshsketchcamera\n");
00175 }
00176 
00177 void Controller::push(ControlBase* c) {
00178   if(!chkCmdStack())
00179     return;
00180   cmdstack.top()->pause();
00181   cmdstack.push(c);
00182   theOneController->gui_comm->printf("push\n");
00183   setNext(cmdstack.top()->activate(display,gui_comm));
00184 }
00185 
00186 void Controller::pop() {
00187   cmdstack.top()->deactivate();
00188   cmdstack.pop();
00189   theOneController->gui_comm->printf("pop\n");
00190   refresh();
00191 }
00192 
00193 Controller& Controller::setRoot(ControlBase* r) {
00194   reset();
00195   root=r;
00196   refresh();
00197   return *this;
00198 }
00199 
00200 Controller& Controller::setEStopID(MotionManager::MC_ID estopid) {
00201   estop_id=estopid;
00202   if(static_cast<EmergencyStopMC*>(motman->peekMotion(estopid))->getStopped()) {
00203     if(!isControlling)
00204       activate();
00205   } else {
00206     if(isControlling)
00207       deactivate();
00208   }   
00209   return *this;
00210 }
00211 
00212 void Controller::loadGUI(const std::string& type, const std::string& name, unsigned int port, const std::vector<std::string>& args) {
00213   if(theOneController==NULL)
00214     return;
00215   std::stringstream ss;
00216   ss << "load\n" << type << '\n' << name << '\n' << port << '\n';
00217   for(unsigned int i=0; i<args.size(); i++) {
00218     ss << '"';
00219     for(unsigned int j=0; j<args[i].size(); j++) {
00220       if(args[i][j]=='\\' || args[i][j]=='"' || args[i][j]=='\n')
00221         ss << '\\';
00222       ss << args[i][j];
00223     }
00224     ss << "\" ";
00225   }
00226   ss << '\n';
00227   theOneController->gui_comm->write((const byte*)ss.str().c_str(),ss.str().size());
00228 }
00229 
00230 void Controller::closeGUI(const std::string& name) {
00231   if(theOneController==NULL)
00232     return;
00233   ASSERTRET(theOneController->gui_comm!=NULL,"null gui_comm");
00234 
00235   theOneController->gui_comm->printf("close\n%s\n",name.c_str());
00236 }
00237 
00238 int Controller::gui_comm_callback(char *buf, int bytes) {
00239   std::string s(buf,bytes);
00240   //  cout << "Controller Received: " << s << endl;
00241   if(theOneController==NULL)
00242     return 0;
00243 
00244   static std::string incomplete;
00245 
00246   //pass a line at a time to the controller
00247   while(s.size()>0) {
00248     std::string::size_type endline=s.find('\n');
00249     if(endline==std::string::npos) {
00250       incomplete+=s;
00251       return 0;
00252     }
00253     
00254     //strip a \r\n or a \n
00255     if(endline>0 && s[endline-1]=='\r')
00256       incomplete+=s.substr(0,endline-1);
00257     else
00258       incomplete+=s.substr(0,endline);
00259     
00260     //is now complete
00261     theOneController->takeLine(incomplete); 
00262     incomplete.erase();
00263     s=s.substr(endline+1);
00264   }
00265   
00266   return 0;
00267 }
00268 
00269 int Controller::console_callback(char *buf, int bytes) {
00270   std::string s(buf,bytes);
00271   //  cout << "Console Received: " << s << endl;
00272   if(theOneController==NULL)
00273     return 0;
00274 
00275   static std::string incomplete;
00276 
00277   //pass a line at a time to the controller
00278   while(s.size()>0) {
00279     std::string::size_type endline=s.find('\n');
00280     if(endline==std::string::npos) {
00281       incomplete+=s;
00282       return 0;
00283     }
00284 
00285     //strip a \r\n or a \n
00286     if(endline>0 && s[endline-1]=='\r')
00287       incomplete+=s.substr(0,endline-1);
00288     else
00289       incomplete+=s.substr(0,endline);
00290     
00291     //is now complete
00292     switch(config->main.consoleMode) {
00293       case Config::main_config::CONTROLLER:
00294         theOneController->takeLine(incomplete); break;
00295       case Config::main_config::TEXTMSG:
00296         erouter->postEvent(TextMsgEvent(incomplete,0)); break;
00297       case Config::main_config::AUTO:
00298         if(wireless->isConnected(theOneController->gui_comm->sock))    
00299           erouter->postEvent(TextMsgEvent(incomplete,0));    
00300         else
00301           theOneController->takeLine(incomplete); 
00302         break;
00303     }
00304     incomplete.erase();
00305     s=s.substr(endline+1);
00306   }
00307   
00308   return 0;
00309 }
00310 
00311 /*! Select which model is running and call initButtons with the appropriate button offsets
00312  *  This could be somewhat simplified by using capabilities.getButtonOffset(), (wouldn't need
00313  *  the ERS2xx case with essentially duplicated ERS210 and ERS220 cases), but this
00314  *  style has the advantage that the symbols are checked by the compiler so there's no
00315  *  chance of a typo in a button name going unnoticed. */
00316 void Controller::init() {
00317   usesButtons=false; // to be reset if initButtons is called
00318   
00319   if(TargetName == ERS2xxInfo::TargetName) {
00320     // compatability mode, see which of the targets is actually running
00321     // Note using ERS2xxInfo namespace to get appropriate offsets!
00322     // could remove duplication with "direct" 210/220 cases below by using something like:
00323     //   capabilities.getButtonOffset(ERS210Info::outputNames[ERS210Info::fooButOffset])
00324     if(RobotName == ERS210Info::TargetName) {
00325       initButtons(666,250,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::ChinButOffset,ERS2xxInfo::BackButOffset);
00326     } else if(RobotName == ERS220Info::TargetName) {
00327       //the 220 doesn't really support "fast" because it's using boolean buttons
00328       //i'm using a "hack" on the 210 because the pressure sensitivity causes status
00329       //events to continually be sent but since this is just on/off, it only gets the
00330       //activate/deactivate.  To fix this, override nextItemFast and prevItemFast with
00331       // timers and do timer management in processEvents()
00332       initButtons(666,50,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailCenterButOffset,ERS2xxInfo::BackButOffset);
00333     } else {
00334       cerr << "Controller: Unsupported 2xx model '" << RobotName << "'!  Appears to have buttons, but Controller doesn't know how to use them." << endl;
00335     }
00336   } else if(RobotName == ERS210Info::TargetName) {
00337     initButtons(666,250,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::ChinButOffset,ERS210Info::BackButOffset);
00338   } else if(RobotName == ERS220Info::TargetName) {
00339     //the 220 doesn't really support "fast" because it's using boolean buttons
00340     //i'm using a "hack" on the 210 because the pressure sensitivity causes status
00341     //events to continually be sent but since this is just on/off, it only gets the
00342     //activate/deactivate.  To fix this, override nextItemFast and prevItemFast with
00343     // timers and do timer management in processEvents()
00344     initButtons(666,50,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailCenterButOffset,ERS220Info::BackButOffset);
00345   } else if(RobotName == ERS7Info::TargetName) {
00346     initButtons(500,25,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::MiddleBackButOffset,ERS7Info::HeadButOffset);
00347   } else if(RobotName == ChiaraInfo::TargetName) {
00348     // doesn't support button navigation
00349   } else {
00350 #ifdef TGT_HAS_BUTTONS
00351     // not that big a deal, don't bother with the warning :-/
00352     //cerr << "Controller: Unsupported model '" << RobotName << "'!  Appears to have buttons, but Controller doesn't know how to use them." << endl;
00353 #endif
00354   }
00355 }
00356 
00357 void Controller::initButtons(unsigned fastTime, unsigned downTime, unsigned nextB, unsigned prevB, unsigned nextFastB, unsigned prevFastB, unsigned selectB, unsigned cancelB) {
00358   nextItem=EventBase(EventBase::buttonEGID,nextB,EventBase::deactivateETID,0);
00359   prevItem=EventBase(EventBase::buttonEGID,prevB,EventBase::deactivateETID,0);
00360   nextItemFast=EventBase(EventBase::buttonEGID,nextFastB,EventBase::statusETID,fastTime);
00361   prevItemFast=EventBase(EventBase::buttonEGID,prevFastB,EventBase::statusETID,fastTime);
00362   selectItem=EventBase(EventBase::buttonEGID,selectB,EventBase::deactivateETID,downTime);
00363   cancel=EventBase(EventBase::buttonEGID,cancelB,EventBase::deactivateETID,downTime);
00364   usesButtons=true;
00365 }
00366 
00367 
00368 bool Controller::select(ControlBase* item, const std::string& name) {
00369   // Depth first
00370   const std::vector<ControlBase*>& slots = item->getSlots();
00371   for(unsigned int i=0; i<slots.size(); i++) {
00372     if (slots[i] != NULL) {
00373       if (slots[i]->getName() == name) { // sensitive to #Name
00374   char in[10];
00375   snprintf(in, 9, "%d", i); in[9]='\0';
00376   ControlBase * ret = item->takeInput(in);
00377   if(ret!=NULL) {
00378     setNext(ret);
00379     return true;
00380   }
00381       } else {
00382   if (select(slots[i], name)) 
00383     return true;
00384       }
00385     }
00386   }
00387   return false;
00388 }
00389 
00390 void Controller::takeLine(const std::string& s) {
00391   //  cout << "RECEIVED: " << s << endl;
00392   if(s.size()==0)
00393     return;
00394   // break s into a vector of arguments
00395   std::vector<std::string> args;
00396   std::vector<unsigned int> offsets;
00397   if(!string_util::parseArgs(s,args,offsets)) {
00398     serr->printf("Controller::takeLine(\"%s\") was malformed.\n",s.c_str());
00399     return;
00400   }
00401   if(args.size()==0 || offsets.size()==0)
00402     return;
00403   // now look through for a ';' (separates multiple commands)
00404   unsigned int last=offsets[0];
00405   for(unsigned int i=0; i<args.size(); i++) {
00406     if(args[i]==";") { // if we found a ';', recurse with substring
00407       takeLine(s.substr(last,offsets[i]-last));
00408       if(i+1==args.size()) // last arg is a ';'
00409         return;
00410       last=offsets[i+1];
00411     }
00412     if(args[i]=="\\;") // if we found a '\;', replace it with base ';'
00413       args[i]=";";
00414   }
00415   if(!chkCmdStack())
00416     return;
00417   if(args[0][0]!='!') {
00418     setNext(cmdstack.top()->takeInput(s));
00419   } else {
00420     if(last!=offsets[0]) { // only changes if we found a ';' - in that case, need to do last segment
00421       takeLine(s.substr(last));
00422     } else if(args[0]=="!refresh") {
00423       refresh();
00424     } else if(args[0]=="!reset") {
00425       reset();
00426     } else if(args[0]=="!cancel") {
00427       setNext(cmdstack.top()->doCancel());
00428     } else if(args[0]=="!select") {
00429       if (args.size() == 1)
00430         setNext(cmdstack.top()->doSelect());
00431       else {
00432         select(root, args[1].c_str());
00433         refresh();
00434       }
00435     } else if(args[0]=="!next") {
00436       setNext(cmdstack.top()->doNextItem());
00437     } else if(args[0]=="!prev") {
00438       setNext(cmdstack.top()->doPrevItem());
00439     } else if(args[0]=="!dump_stack") {
00440       dumpStack();
00441     } else if(args[0]=="!post") {
00442       if(args.size()<4) {
00443         serr->printf("Bad post command, need at least 3 arguments: generator source type [duration]\n");
00444         return;
00445       }
00446       //parse generator id -- could be a generator name or a numeric value
00447       int egid=0;
00448       for(;egid<EventBase::numEGIDs && args[1]!=EventBase::EventGeneratorNames[egid];egid++) {}
00449       if(egid==EventBase::numEGIDs) {
00450         egid=atoi(args[1].c_str());
00451         if(egid==0 && args[1]!="0") {
00452           serr->printf("Bad event generator '%s'\n",args[1].c_str());
00453           return;
00454         }
00455       }
00456       //parse source id -- numeric value, unless egid is buttonEGID, in which case we can look up a button name
00457       //(if you want to add support for other symbolic source types, this is where to do it)
00458       unsigned int source;
00459       if(egid==EventBase::buttonEGID) {
00460         source=capabilities.findButtonOffset(args[2].c_str());
00461         if(source==-1U) {
00462           source=atoi(args[2].c_str());
00463           if(source==0 && args[2]!="0") {
00464             serr->printf("Invalid button name or index '%s'\n",args[2].c_str());
00465             return;
00466           }
00467         }
00468       } else {
00469         source=atoi(args[2].c_str());
00470       }
00471       //parse type id -- numeric, name, or abbreviated name
00472       int etid=0;
00473       for(;etid<EventBase::numETIDs && args[3]!=EventBase::EventTypeNames[etid];etid++) {}
00474       if(etid==EventBase::numETIDs) {
00475         etid=0;
00476         for(;etid<EventBase::numETIDs && args[3]!=EventBase::EventTypeAbbr[etid];etid++) {}
00477         if(etid==EventBase::numETIDs) {
00478           etid=atoi(args[3].c_str());
00479           if(etid==0 && args[3]!="0") {
00480             serr->printf("Bad event type '%s'\n",args[3].c_str());
00481             return;
00482           }
00483         }
00484       }
00485       //duration field (optional, have to check args.size())
00486       int dur=0;
00487       if(args.size()>4)
00488         dur=atoi(args[4].c_str());
00489       //send event!
00490       if(egid==EventBase::buttonEGID && isControlling && usesButtons)
00491         erouter->removeTrapper(this);
00492       erouter->postEvent((EventBase::EventGeneratorID_t)egid,source,(EventBase::EventTypeID_t)etid,dur);
00493       if(egid==EventBase::buttonEGID && isControlling && usesButtons)
00494         erouter->addTrapper(this,EventBase::buttonEGID);
00495     } else if(args[0]=="!msg") {
00496       if(offsets.size()>1)
00497         erouter->postEvent(TextMsgEvent(s.substr(offsets[1]),0));
00498       else
00499         erouter->postEvent(TextMsgEvent("",0));
00500     } else if(args[0]=="!hello") {
00501       static unsigned int count=0;
00502       count++;
00503       theOneController->gui_comm->printf("hello\n%d\n",count);
00504     } else if(args[0]=="!root") {
00505       ControlBase * ret=root->takeInput(s.substr(offsets[1]));
00506       if(ret!=NULL)
00507         setNext(ret);
00508       // for some reason corrupts ControllerGUI's stack, so let's just always send it
00509       dumpStack();
00510     } else if(args[0]=="!hilight") {
00511       std::vector<unsigned int> hilights;
00512       for(unsigned int i=1; i<args.size(); i++)
00513         hilights.push_back(atoi(args[i].c_str()));
00514       cmdstack.top()->setHilights(hilights);
00515     } else if(args[0]=="!input") {
00516       const std::vector<unsigned int>& hilights=cmdstack.top()->getHilights();
00517       const std::vector<ControlBase*>& slots=cmdstack.top()->getSlots();
00518       std::string in=s.substr(offsets[1]);
00519       for(unsigned int i=0; i<hilights.size(); i++)
00520         if(hilights[i]<slots.size() && slots[hilights[i]]!=NULL) {
00521           ControlBase * ret=slots[hilights[i]]->takeInput(in);
00522           if(ret!=NULL)
00523             setNext(ret);
00524         }
00525       refresh();
00526     } else if(args[0]=="!set") {
00527       setConfig(s.substr(offsets[1]).c_str());
00528     } else if(args[0]=="!sim") {
00529 #ifdef PLATFORM_APERIOS
00530       serr->printf("!sim command invalid -- not running in simulator!\n");
00531 #else
00532       ProjectInterface::sendCommand(s.substr(offsets[1]));
00533 #endif
00534     } else
00535       setNext(cmdstack.top()->takeInput(s));
00536   }
00537 }
00538 
00539 void Controller::dumpStack() {
00540   theOneController->gui_comm->printf("stack_dump\n%lu\n",(unsigned long)cmdstack.size());
00541   //this is rather ugly - can't iterate a stack, have to unstack and restack it.  Oh well.
00542   std::stack< ControlBase* > tmpstack;
00543   while(!cmdstack.empty()) {
00544     tmpstack.push(cmdstack.top());
00545     cmdstack.pop();
00546   }
00547   while(!tmpstack.empty()) {
00548     theOneController->gui_comm->printf("%s\n",tmpstack.top()->getName().c_str());
00549     cmdstack.push(tmpstack.top());
00550     tmpstack.pop();
00551   }
00552 } 
00553 
00554 int Controller::setConfig(const std::string& str) {
00555   string::size_type eq=str.find('=');
00556   if(eq==string::npos)
00557     return -2;
00558   plist::ObjectBase* entry = config->resolveEntry(string_util::trim(str.substr(0,eq)));
00559   if(entry==NULL) {
00560     string::size_type p=str.find('.');
00561     string sec=string_util::trim(str.substr(0,p));
00562     string key=string_util::trim(str.substr(p+1,eq-p-1));
00563     string val=string_util::trim(str.substr(eq+1));
00564     if(config->setValue(sec,key,val)==NULL)
00565       return -2;
00566     return 0;
00567   }
00568   plist::PrimitiveBase* prim = dynamic_cast<plist::PrimitiveBase*>(entry);
00569   if(prim==NULL)
00570     return -2;
00571   prim->set(string_util::trim(str.substr(eq+1)));
00572   return 0;
00573 }
00574 
00575 bool Controller::setNext(ControlBase* next) {
00576   if(next==NULL)
00577     pop();
00578   else if(next!=cmdstack.top())
00579     push(next);
00580   return true;
00581 }
00582 
00583 void Controller::activate() {
00584 #ifdef TGT_HAS_LEDS
00585   motman->setPriority(display,MotionManager::kEmergencyPriority);
00586 #endif
00587   if(usesButtons)
00588     erouter->addTrapper(this,EventBase::buttonEGID);
00589   isControlling=true;
00590   if(!cmdstack.empty())
00591     cmdstack.top()->activate(display,gui_comm);
00592   else
00593     chkCmdStack();
00594 }
00595 
00596 void Controller::deactivate() {
00597   //these two lines help prevent residual display in case that was the only MotionCommand using LEDs
00598   isControlling=false;
00599 #ifdef TGT_HAS_LEDS
00600   motman->setPriority(display,MotionManager::kIgnoredPriority);
00601   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00602     motman->setOutput(NULL,i,0.f);
00603 #endif
00604   erouter->removeTrapper(this);
00605   cmdstack.top()->pause();
00606 }
00607 
00608 bool Controller::chkCmdStack() {
00609   if(cmdstack.empty()) {
00610     if(root==NULL)
00611       return false;
00612     cmdstack.push(root);
00613     ControlBase * next = cmdstack.top()->activate(display,gui_comm);
00614     if(next==NULL)
00615       cout << "*** WARNING Controller root returned NULL on activate!" << endl;
00616     else if(next!=root)
00617       push(next);
00618   }
00619   return true;
00620 }
00621 
00622 
00623 /*! @file
00624  * @brief Implements Controller class, a behavior that should be started whenever the emergency stop goes on to provide menus for robot control
00625  * @author ejt (Creator)
00626  */

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:37 2016 by Doxygen 1.6.3