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
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
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) {
00103 if(display==MotionManager::invalid_MC_ID)
00104 activate();
00105 } else {
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
00182 if(theOneController==NULL)
00183 return 0;
00184
00185 static std::string incomplete;
00186
00187
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);
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
00206 if(theOneController==NULL)
00207 return 0;
00208
00209 static std::string incomplete;
00210
00211
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
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
00243
00244
00245
00246
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
00256 if(s.size()==0) {
00257 return;
00258 } else if(s[0]!='!') {
00259 setNext(cmdstack.top()->takeInput(s));
00260 } else {
00261
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
00296
00297
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
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
00367
00368
00369
00370
00371
00372
00373
00374
00375