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 "Shared/ERS7Info.h"
00013 #include "Shared/string_util.h"
00014 #include <sstream>
00015
00016 Controller * Controller::theOneController=NULL;
00017
00018
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
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->addMotion(leds,isControlling?MotionManager::kEmergencyPriority:MotionManager::kIgnoredPriority);
00047 reset();
00048 }
00049
00050 void Controller::DoStop() {
00051 motman->removeMotion(display);
00052
00053 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00054 motman->setOutput(NULL,i,0.f);
00055 display=MotionManager::invalid_MC_ID;
00056 sndman->ReleaseFile(config->controller.select_snd);
00057 sndman->ReleaseFile(config->controller.next_snd);
00058 sndman->ReleaseFile(config->controller.prev_snd);
00059 sndman->ReleaseFile(config->controller.read_snd);
00060 sndman->ReleaseFile(config->controller.cancel_snd);
00061 erouter->forgetListener(this);
00062 reset();
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
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) {
00124 if(!isControlling)
00125 activate();
00126 } else {
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
00212 if(theOneController==NULL)
00213 return 0;
00214
00215 static std::string incomplete;
00216
00217
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
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
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
00243 if(theOneController==NULL)
00244 return 0;
00245
00246 static std::string incomplete;
00247
00248
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
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
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
00286
00287
00288
00289
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 void Controller::takeLine(const std::string& s) {
00307
00308 if(s.size()==0)
00309 return;
00310
00311 std::vector<std::string> args;
00312 std::vector<unsigned int> offsets;
00313 if(!string_util::parseArgs(s,args,offsets)) {
00314 serr->printf("Controller::takeLine(\"%s\") was malformed.\n",s.c_str());
00315 return;
00316 }
00317 if(args.size()==0 || offsets.size()==0)
00318 return;
00319
00320 unsigned int last=offsets[0];
00321 for(unsigned int i=0; i<args.size(); i++) {
00322 if(args[i]==";") {
00323 takeLine(s.substr(last,offsets[i]-last));
00324 if(i+1==args.size())
00325 return;
00326 last=offsets[i+1];
00327 }
00328 if(args[i]=="\\;")
00329 args[i]=";";
00330 }
00331 if(!chkCmdStack())
00332 return;
00333 if(args[0][0]!='!') {
00334 setNext(cmdstack.top()->takeInput(s));
00335 } else {
00336 if(last!=offsets[0]) {
00337 takeLine(s.substr(last));
00338 } else if(args[0]=="!refresh") {
00339 refresh();
00340 } else if(args[0]=="!reset") {
00341 reset();
00342 } else if(args[0]=="!cancel") {
00343 setNext(cmdstack.top()->doCancel());
00344 } else if(args[0]=="!select") {
00345 setNext(cmdstack.top()->doSelect());
00346 } else if(args[0]=="!next") {
00347 setNext(cmdstack.top()->doNextItem());
00348 } else if(args[0]=="!prev") {
00349 setNext(cmdstack.top()->doPrevItem());
00350 } else if(args[0]=="!msg") {
00351 if(offsets.size()>1)
00352 erouter->postEvent(new TextMsgEvent(s.substr(offsets[1])));
00353 else
00354 erouter->postEvent(new TextMsgEvent(""));
00355 } else if(args[0]=="!hello") {
00356 static unsigned int count=0;
00357 count++;
00358 theOneController->gui_comm->printf("hello\n%d\n",count);
00359 } else if(args[0]=="!root") {
00360 ControlBase * ret=root->takeInput(s.substr(offsets[1]));
00361 if(ret!=NULL)
00362 setNext(ret);
00363 } else if(args[0]=="!hilight") {
00364 std::vector<unsigned int> hilights;
00365 for(unsigned int i=1; i<args.size(); i++)
00366 hilights.push_back(atoi(args[i].c_str()));
00367 cmdstack.top()->setHilights(hilights);
00368 } else if(args[0]=="!input") {
00369 const std::vector<unsigned int>& hilights=cmdstack.top()->getHilights();
00370 const std::vector<ControlBase*>& slots=cmdstack.top()->getSlots();
00371 std::string in=s.substr(offsets[1]);
00372 for(unsigned int i=0; i<hilights.size(); i++)
00373 if(hilights[i]<slots.size() && slots[hilights[i]]!=NULL) {
00374 ControlBase * ret=slots[hilights[i]]->takeInput(in);
00375 if(ret!=NULL)
00376 setNext(ret);
00377 }
00378 refresh();
00379 } else if(args[0]=="!set") {
00380 setConfig(s.substr(offsets[1]).c_str());
00381 } else
00382 setNext(cmdstack.top()->takeInput(s));
00383 }
00384 }
00385
00386 int Controller::setConfig(const char *str) {
00387 char buf[80];
00388 strncpy(buf, str, 79);
00389 char *value=index(buf, '=');
00390 char *key=index(buf, '.');
00391 if (key==NULL || value==NULL) return -1;
00392 if (key>=value) return -1;
00393 *key=0;
00394 key++;
00395 *value=0;
00396 value++;
00397 Config::section_t section=config->parseSection(buf);
00398 if (section==Config::sec_invalid) return -2;
00399 config->setValue(section, key, value, true);
00400
00401
00402
00403
00404 return 0;
00405 }
00406
00407 bool Controller::setNext(ControlBase* next) {
00408 if(next==NULL)
00409 pop();
00410 else if(next!=cmdstack.top())
00411 push(next);
00412 return true;
00413 }
00414
00415 void Controller::activate() {
00416 motman->setPriority(display,MotionManager::kEmergencyPriority);
00417 erouter->addTrapper(this,EventBase::buttonEGID);
00418 isControlling=true;
00419 if(!cmdstack.empty())
00420 cmdstack.top()->activate(display,gui_comm);
00421 else
00422 chkCmdStack();
00423 }
00424
00425 void Controller::deactivate() {
00426
00427 motman->setPriority(display,MotionManager::kIgnoredPriority);
00428 isControlling=false;
00429 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00430 motman->setOutput(NULL,i,0.f);
00431 erouter->removeTrapper(this);
00432 cmdstack.top()->pause();
00433 }
00434
00435 bool Controller::chkCmdStack() {
00436 if(cmdstack.empty()) {
00437 if(root==NULL)
00438 return false;
00439 cmdstack.push(root);
00440 ControlBase * next = cmdstack.top()->activate(display,gui_comm);
00441 if(next==NULL)
00442 cout << "*** WARNING Controller root returned NULL on activate!" << endl;
00443 else if(next!=root)
00444 push(next);
00445 }
00446 return true;
00447 }
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459