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