Tekkotsu: Controller.cc Source File

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