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/RobotInfo.h"
00011 #include "Shared/ERS210Info.h"
00012 #include "Shared/ERS220Info.h"
00013 #include "Shared/ERS2xxInfo.h"
00014 #include "Shared/ERS7Info.h"
00015 #include "Shared/string_util.h"
00016 #include "Shared/ProjectInterface.h"
00017 #include <sstream>
00018
00019 Controller * Controller::theOneController=NULL;
00020
00021
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 using namespace std;
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
00041 gui_comm=wireless->socket(Socket::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
00064 #ifdef TGT_HAS_LEDS
00065 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00066 motman->setOutput(NULL,i,0.f);
00067 #endif
00068 gui_comm->printf("goodbye\n");
00069 wireless->setDaemon(gui_comm,false);
00070 wireless->close(gui_comm);
00071 theOneController=NULL;
00072 BehaviorBase::DoStop();
00073 }
00074
00075 bool Controller::trapEvent(const EventBase& e) {
00076 if(!chkCmdStack())
00077 return false;
00078 last_time=cur_time;
00079 cur_time=get_time();
00080
00081 if(state->buttons[nextItem.getSourceID()] && state->buttons[prevItem.getSourceID()] && state->buttons[selectItem.getSourceID()])
00082 return true;
00083
00084 if(nextItem.sameGenSource(e)) {
00085 nextEv_val=e.getMagnitude();
00086 nextEv_dur=e.getDuration();
00087 if(nextEv_val==0 && prevEv_val==0)
00088 alreadyGotBoth=false;
00089 if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666)
00090 if(alreadyGotBoth)
00091 return true;
00092 else {
00093 alreadyGotBoth=true;
00094 return setNext(cmdstack.top()->doReadStdIn());
00095 }
00096 if(e.getTypeID()==nextItem.getTypeID() && e.getDuration()<666)
00097 return setNext(cmdstack.top()->doNextItem());
00098 if(e.getTypeID()==nextItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00099 return setNext(cmdstack.top()->doNextItem());
00100 }
00101 if(prevItem.sameGenSource(e)) {
00102 prevEv_val=e.getMagnitude();
00103 prevEv_dur=e.getDuration();
00104 if(nextEv_val==0 && prevEv_val==0)
00105 alreadyGotBoth=false;
00106 if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666)
00107 if(alreadyGotBoth)
00108 return true;
00109 else {
00110 alreadyGotBoth=true;
00111 return setNext(cmdstack.top()->doReadStdIn());
00112 }
00113 if(e.getTypeID()==prevItem.getTypeID() && e.getDuration()<666)
00114 return setNext(cmdstack.top()->doPrevItem());
00115 if(e.getTypeID()==prevItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00116 return setNext(cmdstack.top()->doPrevItem());
00117 }
00118 if(e.getDuration()>250) {
00119 if(e==selectItem)
00120 return setNext(cmdstack.top()->doSelect());
00121 if(e==cancel)
00122 return setNext(cmdstack.top()->doCancel());
00123 }
00124 return true;
00125 }
00126
00127 void Controller::processEvent(const EventBase& event) {
00128 if(event.getTypeID()==EventBase::activateETID) {
00129 if(!isControlling)
00130 activate();
00131 } else {
00132 if(isControlling)
00133 deactivate();
00134 }
00135 }
00136
00137 void Controller::reset() {
00138 while(cmdstack.size()>1)
00139 pop();
00140 if(!cmdstack.empty()) {
00141 cmdstack.top()->deactivate();
00142 cmdstack.pop();
00143 }
00144 refresh();
00145 }
00146
00147 void Controller::refresh() {
00148 if(!chkCmdStack())
00149 return;
00150 cmdstack.top()->refresh();
00151 }
00152
00153 void Controller::push(ControlBase* c) {
00154 if(!chkCmdStack())
00155 return;
00156 cmdstack.top()->pause();
00157 cmdstack.push(c);
00158 theOneController->gui_comm->printf("push\n");
00159 setNext(cmdstack.top()->activate(display,gui_comm));
00160 }
00161
00162 void Controller::pop() {
00163 cmdstack.top()->deactivate();
00164 cmdstack.pop();
00165 theOneController->gui_comm->printf("pop\n");
00166 refresh();
00167 }
00168
00169 Controller& Controller::setRoot(ControlBase* r) {
00170 reset();
00171 root=r;
00172 refresh();
00173 return *this;
00174 }
00175
00176 Controller& Controller::setEStopID(MotionManager::MC_ID estopid) {
00177 estop_id=estopid;
00178 if(static_cast<EmergencyStopMC*>(motman->peekMotion(estopid))->getStopped()) {
00179 if(!isControlling)
00180 activate();
00181 } else {
00182 if(isControlling)
00183 deactivate();
00184 }
00185 return *this;
00186 }
00187
00188 void Controller::loadGUI(const std::string& type, const std::string& name, unsigned int port, const std::vector<std::string>& args) {
00189 if(theOneController==NULL)
00190 return;
00191 std::stringstream ss;
00192 ss << "load\n" << type << '\n' << name << '\n' << port << '\n';
00193 for(unsigned int i=0; i<args.size(); i++) {
00194 ss << '"';
00195 for(unsigned int j=0; j<args[i].size(); j++) {
00196 if(args[i][j]=='\\' || args[i][j]=='"' || args[i][j]=='\n')
00197 ss << '\\';
00198 ss << args[i][j];
00199 }
00200 ss << "\" ";
00201 }
00202 ss << '\n';
00203 theOneController->gui_comm->write((const byte*)ss.str().c_str(),ss.str().size());
00204 }
00205
00206 void Controller::closeGUI(const std::string& name) {
00207 if(theOneController==NULL)
00208 return;
00209 ASSERTRET(theOneController->gui_comm!=NULL,"null gui_comm");
00210
00211 theOneController->gui_comm->printf("close\n%s\n",name.c_str());
00212 }
00213
00214 int Controller::gui_comm_callback(char *buf, int bytes) {
00215 std::string s(buf,bytes);
00216
00217 if(theOneController==NULL)
00218 return 0;
00219
00220 static std::string incomplete;
00221
00222
00223 while(s.size()>0) {
00224 std::string::size_type endline=s.find('\n');
00225 if(endline==std::string::npos) {
00226 incomplete+=s;
00227 return 0;
00228 }
00229
00230
00231 if(endline>0 && s[endline-1]=='\r')
00232 incomplete+=s.substr(0,endline-1);
00233 else
00234 incomplete+=s.substr(0,endline);
00235
00236
00237 theOneController->takeLine(incomplete);
00238 incomplete.erase();
00239 s=s.substr(endline+1);
00240 }
00241
00242 return 0;
00243 }
00244
00245 int Controller::console_callback(char *buf, int bytes) {
00246 std::string s(buf,bytes);
00247
00248 if(theOneController==NULL)
00249 return 0;
00250
00251 static std::string incomplete;
00252
00253
00254 while(s.size()>0) {
00255 std::string::size_type endline=s.find('\n');
00256 if(endline==std::string::npos) {
00257 incomplete+=s;
00258 return 0;
00259 }
00260
00261
00262 if(endline>0 && s[endline-1]=='\r')
00263 incomplete+=s.substr(0,endline-1);
00264 else
00265 incomplete+=s.substr(0,endline);
00266
00267
00268 switch(config->main.consoleMode) {
00269 case Config::main_config::CONTROLLER:
00270 theOneController->takeLine(incomplete); break;
00271 case Config::main_config::TEXTMSG:
00272 erouter->postEvent(TextMsgEvent(incomplete,0)); break;
00273 case Config::main_config::AUTO:
00274 if(wireless->isConnected(theOneController->gui_comm->sock))
00275 erouter->postEvent(TextMsgEvent(incomplete,0));
00276 else
00277 theOneController->takeLine(incomplete);
00278 break;
00279 }
00280 incomplete.erase();
00281 s=s.substr(endline+1);
00282 }
00283
00284 return 0;
00285 }
00286
00287
00288
00289
00290
00291
00292 void Controller::init() {
00293 if(TargetName == ERS2xxInfo::TargetName) {
00294
00295
00296
00297
00298 if(RobotName == ERS210Info::TargetName) {
00299 initButtons(666,250,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::ChinButOffset,ERS2xxInfo::BackButOffset);
00300 } else if(RobotName == ERS220Info::TargetName) {
00301
00302
00303
00304
00305
00306 initButtons(666,50,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailCenterButOffset,ERS2xxInfo::BackButOffset);
00307 } else {
00308 cerr << "Controller: Unsupported 2xx model '" << RobotName << "'! Appears to have buttons, but Controller doesn't know how to use them." << endl;
00309 }
00310 } else if(RobotName == ERS210Info::TargetName) {
00311 initButtons(666,250,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::ChinButOffset,ERS210Info::BackButOffset);
00312 } else if(RobotName == ERS220Info::TargetName) {
00313
00314
00315
00316
00317
00318 initButtons(666,50,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailCenterButOffset,ERS220Info::BackButOffset);
00319 } else if(RobotName == ERS7Info::TargetName) {
00320 initButtons(500,25,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::MiddleBackButOffset,ERS7Info::HeadButOffset);
00321 } else {
00322 #ifdef TGT_HAS_BUTTONS
00323 cerr << "Controller: Unsupported model '" << RobotName << "'! Appears to have buttons, but Controller doesn't know how to use them." << endl;
00324 #endif
00325 }
00326 }
00327
00328 void Controller::initButtons(unsigned fastTime, unsigned downTime, unsigned nextB, unsigned prevB, unsigned nextFastB, unsigned prevFastB, unsigned selectB, unsigned cancelB) {
00329 nextItem=EventBase(EventBase::buttonEGID,nextB,EventBase::deactivateETID,0);
00330 prevItem=EventBase(EventBase::buttonEGID,prevB,EventBase::deactivateETID,0);
00331 nextItemFast=EventBase(EventBase::buttonEGID,nextFastB,EventBase::statusETID,fastTime);
00332 prevItemFast=EventBase(EventBase::buttonEGID,prevFastB,EventBase::statusETID,fastTime);
00333 selectItem=EventBase(EventBase::buttonEGID,selectB,EventBase::deactivateETID,downTime);
00334 cancel=EventBase(EventBase::buttonEGID,cancelB,EventBase::deactivateETID,downTime);
00335 }
00336
00337
00338 bool Controller::select(ControlBase* item, const std::string& name) {
00339
00340 const std::vector<ControlBase*>& slots = item->getSlots();
00341 for(unsigned int i=0; i<slots.size(); i++) {
00342 if (slots[i] != NULL) {
00343 if (slots[i]->getName() == name) {
00344 char in[10];
00345 snprintf(in, 9, "%d", i); in[9]='\0';
00346 ControlBase * ret = item->takeInput(in);
00347 if(ret!=NULL) {
00348 setNext(ret);
00349 return true;
00350 }
00351 } else {
00352 if (select(slots[i], name))
00353 return true;
00354 }
00355 }
00356 }
00357 return false;
00358 }
00359
00360 void Controller::takeLine(const std::string& s) {
00361
00362 if(s.size()==0)
00363 return;
00364
00365 std::vector<std::string> args;
00366 std::vector<unsigned int> offsets;
00367 if(!string_util::parseArgs(s,args,offsets)) {
00368 serr->printf("Controller::takeLine(\"%s\") was malformed.\n",s.c_str());
00369 return;
00370 }
00371 if(args.size()==0 || offsets.size()==0)
00372 return;
00373
00374 unsigned int last=offsets[0];
00375 for(unsigned int i=0; i<args.size(); i++) {
00376 if(args[i]==";") {
00377 takeLine(s.substr(last,offsets[i]-last));
00378 if(i+1==args.size())
00379 return;
00380 last=offsets[i+1];
00381 }
00382 if(args[i]=="\\;")
00383 args[i]=";";
00384 }
00385 if(!chkCmdStack())
00386 return;
00387 if(args[0][0]!='!') {
00388 setNext(cmdstack.top()->takeInput(s));
00389 } else {
00390 if(last!=offsets[0]) {
00391 takeLine(s.substr(last));
00392 } else if(args[0]=="!refresh") {
00393 refresh();
00394 } else if(args[0]=="!reset") {
00395 reset();
00396 } else if(args[0]=="!cancel") {
00397 setNext(cmdstack.top()->doCancel());
00398 } else if(args[0]=="!select") {
00399 if (args.size() == 1)
00400 setNext(cmdstack.top()->doSelect());
00401 else {
00402 select(root, args[1].c_str());
00403 refresh();
00404 }
00405 } else if(args[0]=="!next") {
00406 setNext(cmdstack.top()->doNextItem());
00407 } else if(args[0]=="!prev") {
00408 setNext(cmdstack.top()->doPrevItem());
00409 } else if(args[0]=="!dump_stack") {
00410 theOneController->gui_comm->printf("stack_dump\n%lu\n",(unsigned long)cmdstack.size());
00411
00412 std::stack< ControlBase* > tmpstack;
00413 while(!cmdstack.empty()) {
00414 tmpstack.push(cmdstack.top());
00415 cmdstack.pop();
00416 }
00417 while(!tmpstack.empty()) {
00418 theOneController->gui_comm->printf("%s\n",tmpstack.top()->getName().c_str());
00419 cmdstack.push(tmpstack.top());
00420 tmpstack.pop();
00421 }
00422 } else if(args[0]=="!post") {
00423 if(args.size()<4) {
00424 serr->printf("Bad post command, need at least 3 arguments: generator source type [duration]\n");
00425 return;
00426 }
00427
00428 int egid=0;
00429 for(;egid<EventBase::numEGIDs && args[1]!=EventBase::EventGeneratorNames[egid];egid++) {}
00430 if(egid==EventBase::numEGIDs) {
00431 egid=atoi(args[1].c_str());
00432 if(egid==0 && args[1]!="0") {
00433 serr->printf("Bad event generator '%s'\n",args[1].c_str());
00434 return;
00435 }
00436 }
00437
00438
00439 unsigned int source;
00440 if(egid==EventBase::buttonEGID) {
00441 source=0;
00442 for(;source<NumButtons && args[2]!=buttonNames[source];source++) {}
00443 if(source==NumButtons) {
00444 source=atoi(args[2].c_str());
00445 if(source==0 && args[2]!="0") {
00446 serr->printf("Invalid button name or index '%s'\n",args[2].c_str());
00447 return;
00448 }
00449 }
00450 } else {
00451 source=atoi(args[2].c_str());
00452 }
00453
00454 int etid=0;
00455 for(;etid<EventBase::numETIDs && args[3]!=EventBase::EventTypeNames[etid];etid++) {}
00456 if(etid==EventBase::numETIDs) {
00457 etid=0;
00458 for(;etid<EventBase::numETIDs && args[3]!=EventBase::EventTypeAbbr[etid];etid++) {}
00459 if(etid==EventBase::numETIDs) {
00460 etid=atoi(args[3].c_str());
00461 if(etid==0 && args[3]!="0") {
00462 serr->printf("Bad event type '%s'\n",args[3].c_str());
00463 return;
00464 }
00465 }
00466 }
00467
00468 int dur=0;
00469 if(args.size()>4)
00470 dur=atoi(args[4].c_str());
00471
00472 if(egid==EventBase::buttonEGID && isControlling)
00473 erouter->removeTrapper(this);
00474 erouter->postEvent((EventBase::EventGeneratorID_t)egid,source,(EventBase::EventTypeID_t)etid,dur);
00475 if(egid==EventBase::buttonEGID && isControlling)
00476 erouter->addTrapper(this,EventBase::buttonEGID);
00477 } else if(args[0]=="!msg") {
00478 if(offsets.size()>1)
00479 erouter->postEvent(TextMsgEvent(s.substr(offsets[1]),0));
00480 else
00481 erouter->postEvent(TextMsgEvent("",0));
00482 } else if(args[0]=="!hello") {
00483 static unsigned int count=0;
00484 count++;
00485 theOneController->gui_comm->printf("hello\n%d\n",count);
00486 } else if(args[0]=="!root") {
00487 ControlBase * ret=root->takeInput(s.substr(offsets[1]));
00488 if(ret!=NULL)
00489 setNext(ret);
00490 } else if(args[0]=="!hilight") {
00491 std::vector<unsigned int> hilights;
00492 for(unsigned int i=1; i<args.size(); i++)
00493 hilights.push_back(atoi(args[i].c_str()));
00494 cmdstack.top()->setHilights(hilights);
00495 } else if(args[0]=="!input") {
00496 const std::vector<unsigned int>& hilights=cmdstack.top()->getHilights();
00497 const std::vector<ControlBase*>& slots=cmdstack.top()->getSlots();
00498 std::string in=s.substr(offsets[1]);
00499 for(unsigned int i=0; i<hilights.size(); i++)
00500 if(hilights[i]<slots.size() && slots[hilights[i]]!=NULL) {
00501 ControlBase * ret=slots[hilights[i]]->takeInput(in);
00502 if(ret!=NULL)
00503 setNext(ret);
00504 }
00505 refresh();
00506 } else if(args[0]=="!set") {
00507 setConfig(s.substr(offsets[1]).c_str());
00508 } else if(args[0]=="!sim") {
00509 #ifdef PLATFORM_APERIOS
00510 serr->printf("!sim command invalid -- not running in simulator!\n");
00511 #else
00512 ProjectInterface::sendCommand(s.substr(offsets[1]));
00513 #endif
00514 } else
00515 setNext(cmdstack.top()->takeInput(s));
00516 }
00517 }
00518
00519 int Controller::setConfig(const std::string& str) {
00520 string::size_type eq=str.find('=');
00521 if(eq==string::npos)
00522 return -2;
00523 plist::ObjectBase* entry = config->resolveEntry(string_util::trim(str.substr(0,eq)));
00524 if(entry==NULL) {
00525 string::size_type p=str.find('.');
00526 string sec=string_util::trim(str.substr(0,p));
00527 string key=string_util::trim(str.substr(p+1,eq-p-1));
00528 string val=string_util::trim(str.substr(eq+1));
00529 if(config->setValue(sec,key,val)==NULL)
00530 return -2;
00531 return 0;
00532 }
00533 plist::PrimitiveBase* prim = dynamic_cast<plist::PrimitiveBase*>(entry);
00534 if(prim==NULL)
00535 return -2;
00536 prim->set(string_util::trim(str.substr(eq+1)));
00537 return 0;
00538 }
00539
00540 bool Controller::setNext(ControlBase* next) {
00541 if(next==NULL)
00542 pop();
00543 else if(next!=cmdstack.top())
00544 push(next);
00545 return true;
00546 }
00547
00548 void Controller::activate() {
00549 motman->setPriority(display,MotionManager::kEmergencyPriority);
00550 erouter->addTrapper(this,EventBase::buttonEGID);
00551 isControlling=true;
00552 if(!cmdstack.empty())
00553 cmdstack.top()->activate(display,gui_comm);
00554 else
00555 chkCmdStack();
00556 }
00557
00558 void Controller::deactivate() {
00559
00560 motman->setPriority(display,MotionManager::kIgnoredPriority);
00561 isControlling=false;
00562 #ifdef TGT_HAS_LEDS
00563 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00564 motman->setOutput(NULL,i,0.f);
00565 #endif
00566 erouter->removeTrapper(this);
00567 cmdstack.top()->pause();
00568 }
00569
00570 bool Controller::chkCmdStack() {
00571 if(cmdstack.empty()) {
00572 if(root==NULL)
00573 return false;
00574 cmdstack.push(root);
00575 ControlBase * next = cmdstack.top()->activate(display,gui_comm);
00576 if(next==NULL)
00577 cout << "*** WARNING Controller root returned NULL on activate!" << endl;
00578 else if(next!=root)
00579 push(next);
00580 }
00581 return true;
00582 }
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594