Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WaypointWalkControl.cc

Go to the documentation of this file.
00001 #include "Shared/RobotInfo.h"
00002 #if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)
00003 
00004 #include "WaypointWalkControl.h"
00005 #include "ToggleControl.h"
00006 #include "NullControl.h"
00007 #include "FileInputControl.h"
00008 #include "StringInputControl.h"
00009 #include "ValueEditControl.h"
00010 #include "Motion/MotionManager.h"
00011 #include "Motion/WaypointWalkMC.h"
00012 #include "Motion/WalkMC.h"
00013 #include "Motion/MMAccessor.h"
00014 #include "Motion/WaypointEngine.h"
00015 #include "Sound/SoundManager.h"
00016 
00017 WaypointWalkControl::WaypointWalkControl()
00018   : ControlBase("WaypointWalkControl","Allows interactive control and execution of a set of waypoints"),
00019     isRunning(false), startstopCtl(NULL), loopCtl(NULL), addEgoWPCtl(NULL),
00020     addOffWPCtl(NULL), addAbsWPCtl(NULL), loadCtl(NULL), saveCtl(NULL),
00021     localizationCtl(NULL), listOffset(0), walk_id(invalid_MC_ID)
00022 {
00023   pushSlot(startstopCtl=new NullControl("Execute","Begin running waypoint list"));
00024   pushSlot(loopCtl=new ToggleControl("Loop Waypoints","When last waypoint is reached, start over"));
00025   pushSlot(addEgoWPCtl=new NullControl("Add Egocentric Waypoint","Appends a new egocentric waypoint (heading and location relative) at the end of the list"));
00026   pushSlot(addOffWPCtl=new NullControl("Add Offset Waypoint","Appends a new offset waypoint (location relative) at the end of the list"));
00027   pushSlot(addAbsWPCtl=new NullControl("Add Absolute Waypoint","Appends a new absolute waypoint at the end of the list"));
00028   pushSlot(loadCtl=new FileInputControl("Load Waypoints","Reads a path from a file",config->portPath(config->motion.root)));
00029   loadCtl->setFilter("*.wyp");
00030   pushSlot(saveCtl=new StringInputControl("Save Waypoints","Writes the current path to a file"));
00031   pushSlot(localizationCtl=new StringInputControl("Drift Error Correction","Enter 3 numbers 'x y a' reprenting current error"));
00032   pushSlot(NULL);
00033   listOffset=slotsSize();
00034 }
00035 
00036 ControlBase *
00037 WaypointWalkControl::activate(MC_ID disp_id, Socket * gui) {
00038   if(walk_id==invalid_MC_ID) {
00039     SharedObject<WaypointWalkMC> walk;
00040     walk_id=motman->addPersistentMotion(walk);
00041   }
00042   return ControlBase::activate(disp_id,gui);
00043 }
00044 
00045 void
00046 WaypointWalkControl::refresh() {
00047   if(saveCtl->getLastInput().size()>0) {
00048     std::string filename=saveCtl->getLastInput();
00049     if(filename.find(".")==std::string::npos)
00050       filename+=".wyp";
00051     std::string path=config->motion.makePath(filename);
00052     sout->printf("Attempting save to %s...\n",path.c_str());
00053     MMAccessor<WaypointWalkMC>(walk_id)->SaveWaypointFile(path.c_str());
00054     saveCtl->clearLastInput();
00055   }
00056   if(loadCtl->getLastInput().size()>0) {
00057     sout->printf("Attempting load from %s...\n",loadCtl->getLastInput().c_str());
00058     MMAccessor<WaypointWalkMC>(walk_id)->LoadWaypointFile(loadCtl->getLastInput().c_str());
00059     loadCtl->clearLastInput();
00060   }
00061   if(localizationCtl->getLastInput().size()>0) {
00062     float x=0,y=0,a=0;
00063     sscanf(localizationCtl->getLastInput().c_str(),"%g %g %g",&x,&y,&a);
00064     MMAccessor<WaypointWalkMC> walk(walk_id);
00065     walk->setCurPos(x+walk->getCurX(),y+walk->getCurY(),a+walk->getCurA());
00066     std::cout << "Position is now " << walk->getCurX() << ' ' << walk->getCurY() << ' ' << walk->getCurA() << std::endl;
00067     localizationCtl->clearLastInput();
00068   }
00069   
00070   MMAccessor<WaypointWalkMC> walk(walk_id);
00071 
00072   loopCtl->setStatus(walk->getIsLooping());
00073 
00074   //rebuild waypoint list
00075 
00076   //clear old entries
00077   for(unsigned int i=listOffset; i<slotsSize(); i++)
00078     delete options[i];
00079   options.resize(listOffset);
00080 
00081   //add new entries
00082   WaypointWalkMC::WaypointList_t& wplist=walk->getWaypointList();
00083   unsigned int wpcnt=1;
00084   for(WaypointWalkMC::WaypointListIter_t it=wplist.begin(); it!=wplist.end(); it++) {
00085     char cname[50];
00086     sprintf(cname,"Waypoint %d",wpcnt++);
00087     char desc[100];
00088     const char * pt=NULL;
00089     switch(it->posType) {
00090     case Waypoint::POSTYPE_EGOCENTRIC:
00091       pt="^="; break; //uhh, the '^' is supposed to imply it's heading-relative :-}
00092     case Waypoint::POSTYPE_OFFSET:
00093       pt="+="; break;
00094     case Waypoint::POSTYPE_ABSOLUTE:
00095       pt="="; break;
00096     }
00097     sprintf(desc,"x%s%g, y%s%g, a%s%g, arc=%g", pt,it->x,pt,it->y,
00098             (it->angleIsRelative?"+=":"="),it->angle,it->arc);
00099     pushSlot(new WaypointEditControl(cname,desc,walk_id,it));
00100   }
00101   ControlBase::refresh();
00102 }
00103 
00104 void
00105 WaypointWalkControl::deactivate() {
00106   motman->removeMotion(walk_id);
00107   walk_id=invalid_MC_ID;
00108   ControlBase::deactivate();
00109 }
00110 
00111 ControlBase*
00112 WaypointWalkControl::doSelect() {
00113   for(unsigned int i=0; i<hilights.size(); i++) {
00114     ControlBase * curctl=options[hilights[i]];
00115     if(curctl==startstopCtl) {
00116       if(isRunning) {
00117         isRunning=false;
00118         startstopCtl->setName("Execute");
00119         startstopCtl->setDescription("Begin running waypoint list");
00120         MMAccessor<WaypointWalkMC>(walk_id)->pause();
00121       } else {
00122         isRunning=true;
00123         startstopCtl->setName("Stop");
00124         startstopCtl->setDescription("Halt locomotion");
00125         MMAccessor<WaypointWalkMC>(walk_id)->go();
00126       }
00127       sndman->playFile(config->controller.select_snd);
00128       return curctl;
00129     } else if(curctl==loopCtl) {
00130       MMAccessor<WaypointWalkMC>(walk_id)->setIsLooping(!loopCtl->getStatus());
00131       sndman->playFile(config->controller.select_snd);
00132       return curctl;
00133     } else if(curctl==addEgoWPCtl) {
00134       MMAccessor<WaypointWalkMC>(walk_id)->addEgocentricWaypoint(0,0,false,true,100.f);
00135       sndman->playFile(config->controller.select_snd);
00136       return curctl;
00137     } else if(curctl==addOffWPCtl) {
00138       MMAccessor<WaypointWalkMC>(walk_id)->addOffsetWaypoint(0,0,false,true,100.f);
00139       sndman->playFile(config->controller.select_snd);
00140       return curctl;
00141     } else if(curctl==addAbsWPCtl) {
00142       MMAccessor<WaypointWalkMC>(walk_id)->addAbsoluteWaypoint(0,0,false,true,100.f);
00143       sndman->playFile(config->controller.select_snd);
00144       return curctl;
00145     }
00146   }
00147   return ControlBase::doSelect();
00148 }
00149   
00150 WaypointWalkControl::WaypointEditControl::WaypointEditControl(const std::string& n, const std::string& d, MC_ID walkid, WaypointEngine::WaypointListIter_t waypointid)
00151   : ControlBase(n,d), walk_id(walkid), waypoint_id(waypointid), up(NULL), down(NULL), del(NULL), set(NULL)
00152 {
00153   pushSlot(up=new NullControl("Up (Move up)","Moves up in waypoint list"));
00154   pushSlot(down=new NullControl("Down (Move down)","Moves down in waypoint list"));
00155   pushSlot(del=new NullControl("Delete","Removes from waypoint list"));
00156   pushSlot(set=new NullControl("Set as current goal","Starts trying to reach this location"));
00157   pushSlot(NULL);
00158   MMAccessor<WaypointWalkMC> walk(walk_id);
00159   Waypoint& curway=*waypoint_id;
00160   pushSlot(new ValueEditControl<float>("X",&curway.x));
00161   pushSlot(new ValueEditControl<float>("Y",&curway.y));
00162   pushSlot(new ValueEditControl<float>("A",&curway.angle));
00163   pushSlot(new ValueEditControl<float>("Arc",&curway.arc));
00164   pushSlot(new ValueEditControl<float>("Speed (in mm/s)",&curway.speed));
00165   pushSlot(new ValueEditControl<float>("Turn Speed (in rad/s)",&curway.turnSpeed));
00166   char desc[256];
00167   snprintf(desc,256,"Types: EGO=%d, OFF=%d, ABS=%d",Waypoint::POSTYPE_EGOCENTRIC,Waypoint::POSTYPE_OFFSET,Waypoint::POSTYPE_ABSOLUTE);
00168   pushSlot(new ValueEditControl<Waypoint::posType_t>("Pos. coord. system",desc,&curway.posType));
00169   ToggleControl * tmptgl;
00170   pushSlot(tmptgl=new ToggleControl("Angle is relative"));
00171   tmptgl->setStatus(curway.angleIsRelative);
00172   tmptgl->setStore(&curway.angleIsRelative);
00173   pushSlot(tmptgl=new ToggleControl("Track path"));
00174   tmptgl->setStatus(curway.trackPath);
00175   tmptgl->setStore(&curway.trackPath);
00176 }
00177 
00178 ControlBase* WaypointWalkControl::WaypointEditControl::doSelect() {
00179   for(unsigned int i=0; i<hilights.size(); i++) {
00180     ControlBase * curctl=options[hilights[i]];
00181     if(curctl==up) {
00182       std::swap(*waypoint_id,*(waypoint_id-1));
00183       sndman->playFile(config->controller.select_snd);
00184       return NULL;
00185     } else if(curctl==down) {
00186       std::swap(*waypoint_id,*(waypoint_id+1));
00187       sndman->playFile(config->controller.select_snd);
00188       return NULL;
00189     } else if(curctl==del) {
00190       MMAccessor<WaypointWalkMC>(walk_id)->getWaypointList().erase(waypoint_id);
00191       sndman->playFile(config->controller.select_snd);
00192       return NULL;
00193     } else if(curctl==set) {
00194       MMAccessor<WaypointWalkMC>(walk_id)->setTargetWaypoint(waypoint_id);
00195       sndman->playFile(config->controller.select_snd);
00196       return NULL;
00197     }
00198   }
00199   return ControlBase::doSelect();
00200 }
00201 
00202 
00203     /* //basic
00204       walk->addEgocentricWaypoint(1,0,0,true,.1);
00205       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1);
00206       walk->addEgocentricWaypoint(1,0,0,true,.1);
00207       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1);
00208       walk->addEgocentricWaypoint(1,0,0,true,.1);
00209       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1);
00210       walk->addEgocentricWaypoint(1,0,0,true,.1);
00211       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1);
00212     */
00213 
00214     /*
00215       walk->addEgocentricWaypoint(1,0,0,true,.1);
00216       walk->addEgocentricWaypoint(0,1,-M_PI/2,true,.1);
00217       walk->addEgocentricWaypoint(-1,0,0,false,.1);
00218       walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.1);
00219       walk->addEgocentricWaypoint(1,0,0,true,.1);
00220       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1);
00221     */
00222 
00223     /*
00224       walk->addEgocentricWaypoint(1,0,0,true,.1);
00225       walk->addEgocentricWaypoint(0,1,0,true,.1);
00226       walk->addEgocentricWaypoint(0,1,-M_PI/2,true,.1);
00227       walk->addEgocentricWaypoint(-1,0,M_PI/2,false,.1);
00228       walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.1);
00229     */
00230 
00231     /* //circle1
00232       walk->addEgocentricArc(0,.5,0,true,.1,M_PI);
00233       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1);
00234       walk->addEgocentricArc(.5,0,M_PI/2,true,.1,M_PI);
00235       walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.1);
00236     */
00237 
00238     /* //circle2
00239       walk->addEgocentricArc(0,.25,0,true,.1,M_PI);
00240       walk->addEgocentricWaypoint(0,0,M_PI/2,true,.075);
00241       walk->addEgocentricArc(.25,0,M_PI/2,true,.1,M_PI);
00242       walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.075);
00243     */
00244     
00245 
00246 
00247 /*! @file
00248  * @brief Implements WaypointWalkControl, which allows interactive control and execution of a set of waypoints 
00249  * @author ejt (Creator)
00250  */
00251 
00252 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:52 2016 by Doxygen 1.6.3