Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WaypointWalkControl.cc

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

Tekkotsu v4.0
Generated Thu Nov 22 00:54:56 2007 by Doxygen 1.5.4