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

Tekkotsu v2.2
Generated Tue Oct 19 14:19:16 2004 by Doxygen 1.3.9.1