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

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:41 2004 by Doxygen 1.3.9.1