Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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 "Shared/SharedObject.h"
00006 #include "Shared/WorldState.h"
00007 #include "Shared/get_time.h"
00008 #include "SoundPlay/SoundManager.h"
00009 #include "Events/TextMsgEvent.h"
00010 #include "Shared/ERS210Info.h"
00011 #include "Shared/ERS220Info.h"
00012 #include <sstream>
00013 
00014 Controller * Controller::theOneController=NULL;
00015 
00016 //these are given appropriate values in init once we know which model we're running on
00017 EventBase Controller::nextItem;
00018 EventBase Controller::prevItem;
00019 EventBase Controller::nextItemFast;
00020 EventBase Controller::prevItemFast;
00021 EventBase Controller::selectItem;
00022 EventBase Controller::cancel;
00023 
00024 
00025 void Controller::DoStart() {
00026   BehaviorBase::DoStart();
00027   sndman->LoadFile(config->controller.select_snd);
00028   sndman->LoadFile(config->controller.next_snd);
00029   sndman->LoadFile(config->controller.prev_snd);
00030   sndman->LoadFile(config->controller.read_snd);
00031   sndman->LoadFile(config->controller.cancel_snd);
00032   erouter->addListener(this,EventBase::estopEGID);
00033   // Turn on wireless
00034   wireless->setReceiver(gui_comm->sock, gui_comm_callback);
00035   wireless->listen(gui_comm->sock, config->controller.gui_port);
00036   theOneController=this;
00037   reset();
00038 }
00039 
00040 void Controller::DoStop() {
00041   sndman->ReleaseFile(config->controller.select_snd);
00042   sndman->ReleaseFile(config->controller.next_snd);
00043   sndman->ReleaseFile(config->controller.prev_snd);
00044   sndman->ReleaseFile(config->controller.read_snd);
00045   sndman->ReleaseFile(config->controller.cancel_snd);
00046   erouter->forgetListener(this);
00047   reset();
00048   wireless->close(gui_comm);
00049   theOneController=NULL;
00050   BehaviorBase::DoStop();
00051 }
00052 
00053 bool Controller::trapEvent(const EventBase& e) {
00054   if(!chkCmdStack())
00055     return false;
00056   last_time=cur_time;
00057   cur_time=get_time();
00058   if(nextItem.sameGenSource(e)) {
00059     nextEv_val=e.getMagnitude();
00060     nextEv_dur=e.getDuration();
00061     if(nextEv_val==0 && prevEv_val==0)
00062       alreadyGotBoth=false;
00063     if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666)
00064       if(alreadyGotBoth)
00065         return true;
00066       else {
00067         alreadyGotBoth=true;
00068         return setNext(cmdstack.top()->doReadStdIn());
00069       }
00070     if(e.getTypeID()==nextItem.getTypeID() && e.getDuration()<666)
00071       return setNext(cmdstack.top()->doNextItem());
00072     if(e.getTypeID()==nextItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00073       return setNext(cmdstack.top()->doNextItem());
00074   }
00075   if(prevItem.sameGenSource(e)) {
00076     prevEv_val=e.getMagnitude();
00077     prevEv_dur=e.getDuration();
00078     if(nextEv_val==0 && prevEv_val==0)
00079       alreadyGotBoth=false;
00080     if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666)
00081       if(alreadyGotBoth)
00082         return true;
00083       else {
00084         alreadyGotBoth=true;
00085         return setNext(cmdstack.top()->doReadStdIn());
00086       }
00087     if(e.getTypeID()==prevItem.getTypeID() && e.getDuration()<666)
00088       return setNext(cmdstack.top()->doPrevItem());
00089     if(e.getTypeID()==prevItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00090       return setNext(cmdstack.top()->doPrevItem());
00091   }
00092   if(e.getDuration()>250) {
00093     if(e==selectItem)
00094       return setNext(cmdstack.top()->doSelect());
00095     if(e==cancel)
00096       return setNext(cmdstack.top()->doCancel());
00097   }
00098   return true;
00099 }
00100 
00101 void Controller::processEvent(const EventBase& event) {
00102   if(event.getTypeID()==EventBase::activateETID) { //estop just turned on
00103     if(display==MotionManager::invalid_MC_ID)
00104       activate();
00105   } else { //estop just turned off
00106     if(display!=MotionManager::invalid_MC_ID)
00107       deactivate();
00108   }
00109 }
00110 
00111 void Controller::reset() {
00112   while(cmdstack.size()>1)
00113     pop();
00114   if(!cmdstack.empty()) {
00115     cmdstack.top()->deactivate();
00116     cmdstack.pop();
00117   }
00118 }
00119 
00120 void Controller::refresh() {
00121   if(!chkCmdStack())
00122     return;
00123   cmdstack.top()->refresh();
00124 }
00125 
00126 void Controller::push(ControlBase* c) {
00127   if(!chkCmdStack())
00128     return;
00129   cmdstack.top()->pause();
00130   cmdstack.push(c);
00131   setNext(cmdstack.top()->activate(display,gui_comm));
00132 }
00133 
00134 void Controller::pop() {
00135   cmdstack.top()->deactivate();
00136   cmdstack.pop();
00137   refresh();
00138 }
00139 
00140 Controller& Controller::setRoot(ControlBase* r) {
00141   reset();
00142   root=r;
00143   refresh();
00144   return *this;
00145 }
00146 
00147 Controller& Controller::setEStopID(MotionManager::MC_ID estopid) {
00148   estop_id=estopid;
00149   if(static_cast<EmergencyStopMC*>(motman->peekMotion(estopid))->getStopped()) {
00150     if(display==MotionManager::invalid_MC_ID)
00151       activate();
00152   } else {
00153     if(display!=MotionManager::invalid_MC_ID)
00154       deactivate();
00155   }   
00156   return *this;
00157 }
00158 
00159 void Controller::loadGUI(const std::string& type, const std::string& name, unsigned int port, const std::vector<std::string>& args) {
00160   std::stringstream ss;
00161   ss << "load\n" << type << '\n' << name << '\n' << port << '\n';
00162   for(unsigned int i=0; i<args.size(); i++) {
00163     ss << '"';
00164     for(unsigned int j=0; j<args[i].size(); j++) {
00165       if(args[i][j]=='\\' || args[i][j]=='"' || args[i][j]=='\n')
00166         ss << '\\';
00167       ss << args[i][j];
00168     }
00169     ss << "\" ";
00170   }
00171   ss << '\n';
00172   theOneController->gui_comm->write((const byte*)ss.str().c_str(),ss.str().size());
00173 }
00174 
00175 void Controller::closeGUI(const std::string& name) {
00176   theOneController->gui_comm->printf("close\n%s\n",name.c_str());
00177 }
00178 
00179 int Controller::gui_comm_callback(char *buf, int bytes) {
00180   std::string s(buf,bytes);
00181   //  cout << "Controller Received: " << s << endl;
00182   if(theOneController==NULL)
00183     return 0;
00184 
00185   static std::string incomplete;
00186 
00187   //pass a line at a time to the controller
00188   while(s.size()>0) {
00189     unsigned int endline=s.find('\n');
00190     if(endline==std::string::npos) {
00191       incomplete+=s;
00192       return 0;
00193     }
00194     incomplete+=s.substr(0,endline);
00195     theOneController->takeLine(incomplete); //is now complete
00196     incomplete.erase();
00197     s=s.substr(endline+1);
00198   }
00199   
00200   return 0;
00201 }
00202 
00203 int Controller::console_callback(char *buf, int bytes) {
00204   std::string s(buf,bytes);
00205   //  cout << "Console Received: " << s << endl;
00206   if(theOneController==NULL)
00207     return 0;
00208 
00209   static std::string incomplete;
00210 
00211   //pass a line at a time to the controller
00212   while(s.size()>0) {
00213     unsigned int endline=s.find('\n');
00214     if(endline==std::string::npos) {
00215       incomplete+=s;
00216       return 0;
00217     }
00218     incomplete+=s.substr(0,endline);
00219     //is now complete:
00220     if(wireless->isConnected(theOneController->gui_comm->sock))
00221       erouter->postEvent(new TextMsgEvent(incomplete));
00222     else
00223       theOneController->takeLine(incomplete); 
00224     incomplete.erase();
00225     s=s.substr(endline+1);
00226   }
00227   
00228   return 0;
00229 }
00230 
00231 void Controller::init() {
00232   if(state->robotDesign & WorldState::ERS210Mask) {
00233     nextItem=EventBase(EventBase::buttonEGID,ERS210Info::HeadFrButOffset,EventBase::deactivateETID,0);
00234     prevItem=EventBase(EventBase::buttonEGID,ERS210Info::HeadBkButOffset,EventBase::deactivateETID,0);
00235     nextItemFast=EventBase(EventBase::buttonEGID,ERS210Info::HeadFrButOffset,EventBase::statusETID,666);
00236     prevItemFast=EventBase(EventBase::buttonEGID,ERS210Info::HeadBkButOffset,EventBase::statusETID,666);
00237     selectItem=EventBase(EventBase::buttonEGID,ERS210Info::ChinButOffset,EventBase::deactivateETID,250);
00238     cancel=EventBase(EventBase::buttonEGID,ERS210Info::BackButOffset,EventBase::deactivateETID,250);
00239   } else if(state->robotDesign & WorldState::ERS220Mask) {
00240     nextItem=EventBase(EventBase::buttonEGID,ERS220Info::TailLeftButOffset,EventBase::deactivateETID,0);
00241     prevItem=EventBase(EventBase::buttonEGID,ERS220Info::TailRightButOffset,EventBase::deactivateETID,0);
00242     //the 220 doesn't really support the next two because it's using boolean buttons
00243     //i'm using a "hack" on the 210 because the pressure sensitivity causes status
00244     //events to continually be sent but since this is just on/off, it only gets the
00245     //activate/deactivate.  To fix this, make these timers and do timer management
00246     //in processEvents()
00247     nextItemFast=EventBase(EventBase::buttonEGID,ERS220Info::TailLeftButOffset,EventBase::statusETID,666);
00248     prevItemFast=EventBase(EventBase::buttonEGID,ERS220Info::TailRightButOffset,EventBase::statusETID,666);
00249     selectItem=EventBase(EventBase::buttonEGID,ERS220Info::TailCenterButOffset,EventBase::deactivateETID,50);
00250     cancel=EventBase(EventBase::buttonEGID,ERS220Info::BackButOffset,EventBase::deactivateETID,50);
00251   }
00252 }
00253 
00254 void Controller::takeLine(const std::string& s) {
00255   //  cout << "RECEIVED: " << s << endl;
00256   if(s.size()==0) {
00257     return;
00258   } else if(s[0]!='!') {
00259     setNext(cmdstack.top()->takeInput(s));
00260   } else {
00261     //find the first non-whitespace character after the '!'
00262     std::string msg;
00263     { unsigned int i=1; while(isspace(s[i]) && i<s.size()) i++; msg=s.substr(i); }
00264     if(msg.find("refresh")==0) {
00265       refresh();
00266     } else if(msg.find("reset")==0) {
00267       reset();
00268     } else if(msg.find("cancel")==0) {
00269       setNext(cmdstack.top()->doCancel());
00270     } else if(msg.find("select")==0) {
00271       setNext(cmdstack.top()->doSelect());
00272     } else if(msg.find("next")==0) {
00273       setNext(cmdstack.top()->doNextItem());
00274     } else if(msg.find("prev")==0) {
00275       setNext(cmdstack.top()->doPrevItem());
00276     } else if(msg.find("msg ")==0) {
00277       erouter->postEvent(new TextMsgEvent(msg.substr(strlen("msg "))));
00278     } else if(msg.find("hilight")==0) {
00279       std::vector<unsigned int> hilights;
00280       unsigned int i=strlen("hilight");
00281       while(i<msg.size()) {
00282         while(i<msg.size() && isspace(msg[i])) i++;
00283         if(i<msg.size())
00284            hilights.push_back(atoi(&msg.c_str()[i]));
00285         while(i<msg.size() && !isspace(msg[i])) i++;
00286       }
00287       cmdstack.top()->setHilights(hilights);
00288     } else if(msg.find("input ")==0) {
00289       const std::vector<unsigned int>& hilights=cmdstack.top()->getHilights();
00290       const std::vector<ControlBase*>& slots=cmdstack.top()->getSlots();
00291       std::string in=msg.substr(strlen("input "));
00292       for(unsigned int i=0; i<hilights.size(); i++) {
00293         if(hilights[i]<slots.size() && slots[hilights[i]]!=NULL)
00294           slots[hilights[i]]->takeInput(in);
00295         /* if(ret!=NULL) {
00296            push(ret);
00297            break;
00298            }*/
00299       }
00300       refresh();
00301     } else
00302       setNext(cmdstack.top()->takeInput(s));
00303   }
00304 }
00305 
00306 bool Controller::setNext(ControlBase* next) {
00307   if(next==NULL)
00308     pop();
00309   else if(next!=cmdstack.top())
00310     push(next);
00311   return true;
00312 }
00313 
00314 void Controller::activate() {
00315   SharedObject<LedMC> leds;
00316   leds->setWeights(~FaceLEDMask,0);
00317   leds->setWeights(FaceLEDMask,.75);
00318   display=motman->addMotion(leds,MotionManager::kEmergencyPriority);
00319   erouter->addTrapper(this,EventBase::buttonEGID);
00320   if(!cmdstack.empty())
00321     cmdstack.top()->activate(display,gui_comm);
00322   else
00323     chkCmdStack();
00324 }
00325 
00326 void Controller::deactivate() {
00327   motman->removeMotion(display);
00328   //these two lines help prevent residual display in case that was the only MotionCommand using LEDs
00329   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00330     motman->setOutput(NULL,i,0.f);
00331   display=MotionManager::invalid_MC_ID;
00332   erouter->removeTrapper(this);
00333   cmdstack.top()->pause();
00334 }
00335 
00336 bool Controller::chkCmdStack() {
00337   if(cmdstack.empty()) {
00338     if(root==NULL)
00339       return false;
00340     cmdstack.push(root);
00341     ControlBase * next = cmdstack.top()->activate(display,gui_comm);
00342     if(next==NULL)
00343       cout << "*** WARNING Controller root returned NULL on activate!" << endl;
00344     else if(next!=root)
00345       push(next);
00346   }
00347   return true;
00348 }
00349 
00350 std::string Controller::makeLower(const std::string& s) {
00351   std::string ans;
00352   ans.reserve(s.size());
00353   unsigned int i=s.size();
00354   while(i--!=0)
00355     ans+= ::tolower(s[i]);
00356 return ans;
00357 }
00358 
00359 std::string Controller::removePrefix(const std::string& str, const std::string& pre) {
00360   if(str.compare(0,pre.size(),pre)==0)
00361     return str.substr(pre.size());
00362   return std::string();
00363 }
00364 
00365 
00366 /*! @file
00367  * @brief Implements Controller class, a behavior that should be started whenever the emergency stop goes on to provide menus for robot control
00368  * @author ejt (Creator)
00369  *
00370  * $Author: ejt $
00371  * $Name: tekkotsu-1_4_1 $
00372  * $Revision: 1.20 $
00373  * $State: Exp $
00374  * $Date: 2003/07/07 01:00:07 $
00375  */

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2