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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:46 2005 by Doxygen 1.4.4