Homepage Demos Overview Downloads Tutorials Reference
Credits

WaypointEngine.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_WaypointEngine_h_
00003 #define INCLUDED_WaypointEngine_h_
00004 
00005 #include "Shared/ListMemBuf.h"
00006 #include "Shared/LoadSave.h"
00007 
00008 //! Provides computation and management of a desired path through a series of waypoints
00009 /*! This is a generalized set of data structures and management code -
00010  *  it doesn't actually say anything about @e how you get from one
00011  *  waypoint to the other, it will just tell you where you should be
00012  *  going at any given time.
00013  *
00014  *  So, for instance, you may be interested in WaypointWalk, which
00015  *  will use a WalkMC to traverse the waypoints.  Future development
00016  *  may include a WaypointPush, to push an object along a path instead
00017  *  of just moving the body along a path.
00018  *
00019  *  Although general curves between waypoints are not supported, you
00020  *  can use either circular arcs or straight lines.
00021  *
00022  *  The Waypoint class holds the actual data about each waypoint.  You
00023  *  can specify waypoints in 3 ways, egocentric, offset, and absolute.
00024  *  - Egocentric: the x and y parameters are relative to the body
00025  *    itself; x is always forward and y is always left.  Handy for
00026  *    turtle/logo style specification of a series of instructions which
00027  *    specify the waypoints.
00028  *  - Offset: the x and y parameters are relative to the current body
00029  *    position, but not its heading.
00030  *  - Absolute: the x and y parameters are direct coordinates
00031  *
00032  *  In addition, you can specify how to handle the heading of the body
00033  *  while the path is executing.  You can specify a relative offset to
00034  *  the direction of travel (for instance, to walk sideways instead of
00035  *  forward), or an absolute value (to always face a certain direction
00036  *  while traveling)
00037  *
00038  *  Dead reckoning is very prone to accruing error.  It is highly
00039  *  recommended that you calibrate the locomotion mechanism carefully
00040  *  and implement some form of localization to handle the inevitable
00041  *  drift.
00042  *
00043  *  Waypoint list files are a fairly straightforward plain text format.
00044  *  The extension .wyp is suggested.
00045  *
00046  *  The format is:
00047  *  - '<tt>#WyP</tt>' - header to verify file type
00048  *  - A series of entries:
00049  *    - '<tt>max_turn_speed </tt><i>num</i>' - sets the maximum error-correction turning speed used for all following waypoints
00050  *    - '<tt>track_path </tt><i>bool</i>' - sets trackpath mode on or off for all following waypoints (see Waypoint::trackPath)
00051  *    - '<tt>add_point </tt>{<tt>ego</tt>|<tt>off</tt>|<tt>abs</tt>}<tt> </tt><i>x_val</i><tt> </tt><i>y_val</i><tt> </tt>{<tt>hold</tt>|<tt>follow</tt>}<tt> </tt><i>angle_val</i><tt> </tt><i>speed_val</i><tt> </tt><i>arc_val</i>' - adds the waypoint with the parameters given, see Waypoint, similar to add*Waypoint functions
00052  *    - '<tt>add_arc </tt>{<tt>ego</tt>|<tt>off</tt>|<tt>abs</tt>}<tt> </tt><i>x_val</i><tt> </tt><i>y_val</i><tt> </tt>{<tt>hold</tt>|<tt>follow</tt>}<tt> </tt><i>angle_val</i><tt> </tt><i>speed_val</i><tt> </tt><i>arc_val</i>' - adds the waypoint with the parameters given, see Waypoint, similar to add*Arc functions
00053  *  - '<tt>#END</tt>' - footer to verify ending of file
00054  */
00055 template<unsigned int MAX_WAY>
00056 class WaypointEngine : public LoadSave {
00057 public:
00058   static const unsigned int MAX_WAYPOINTS=MAX_WAY; //!< for external access to maximum waypoints
00059 
00060   //! Holds information about each waypoint, see WaypointEngine for overview
00061   struct Waypoint {
00062     //! defines different ways to interpret the position values
00063     enum posType_t {
00064       POSTYPE_EGOCENTRIC, //!< #x and #y are relative to current heading - so x is forward and y is strafe
00065       POSTYPE_OFFSET, //!< #x and #y are oriented with the coordinates, but relative to current location (delta x and delta y)
00066       POSTYPE_ABSOLUTE //!< #x and #y are a specific coordinate location
00067     };
00068     Waypoint()
00069       : x(0), y(0), angle(0), arc(), speed(), turnSpeed(), posType(), angleIsRelative(), trackPath()
00070     {} //!< constructor
00071     Waypoint(float xc, float yc, Waypoint::posType_t pos_rel, float ac, bool ang_rel, float spd, bool track, float turn)
00072       : x(xc), y(yc), angle(ac), arc(0), speed(spd), turnSpeed(turn), posType(pos_rel), angleIsRelative(ang_rel), trackPath(track)
00073     {} //!< constructor
00074     float x; //!< the displacement along x (meters), subject to #posType
00075     float y; //!< the displacement along y (meters), subject to #posType
00076     float angle; //!< either the angle relative to path to maintain, or the heading to maintain, see #angleIsRelative
00077     float arc; //!< angle of sector of arc to use to get to waypoint (0 means straight line)
00078     float speed; //!< speed (in meters per second)
00079     float turnSpeed; //!< maximum speed to correct heading (in radians per second)
00080     posType_t posType; //!< lets us know how to interpret the #x and #y values
00081     bool angleIsRelative; //!< if true, #angle is interpreted as relative to the path; otherwise, interpreted as an absolute heading to maintain
00082     bool trackPath; //!< if true, if off course, will attempt to get back on path at the ideal location; if false, simply heads directly for waypoint from whereever it is
00083   };
00084 
00085   typedef ListMemBuf<Waypoint,MAX_WAYPOINTS> WaypointList_t; //!< convenient shorthand
00086   typedef typename ListMemBuf<Waypoint,MAX_WAYPOINTS>::index_t WaypointListIter_t; //!< convenient shorthand
00087 
00088   //! constructor
00089   WaypointEngine()
00090     : LoadSave(), waypoints(), isRunning(false), isLooping(true), isTracking(false),
00091       curWaypoint(waypoints.end()), waypointTime(0), waypointDistance(0), pathLength(0), arcRadius(0),
00092       lastUpdateTime(0), Pcorr(.5), turnSpeed(.65)
00093   {init();}
00094   //! constructor
00095   WaypointEngine(char * f)
00096     : LoadSave(), waypoints(), isRunning(false), isLooping(true), isTracking(false),
00097       curWaypoint(waypoints.end()), waypointTime(0), waypointDistance(0), pathLength(0), arcRadius(0),
00098       lastUpdateTime(0), Pcorr(.5), turnSpeed(.65)
00099   {init(); LoadFile(f); }
00100 
00101   //! returns a rough overestimate of the size needed
00102   /*! pretends we need to switch max_turn_speed and track_path on
00103     every point, and the longest options are given for every point */
00104   virtual unsigned int getBinSize() const;
00105   virtual unsigned int LoadBuffer(const char buf[], unsigned int len);
00106   virtual unsigned int SaveBuffer(char buf[], unsigned int len) const;
00107 
00108   virtual void go();      //!< starts walking towards the first waypoint
00109   virtual void pause();   //!< halts execution of waypoint list
00110   virtual void unpause(); //!< resumes execution of waypoint list from last paused location
00111 
00112   virtual void setIsLooping(bool isl) { isLooping=isl; } //!< sets #isLooping
00113   virtual bool getIsLooping() const { return isLooping; } //!< returns #isLooping
00114 
00115   virtual WaypointList_t& getWaypointList() { return waypoints; } //!< returns a reference to #waypoints
00116   virtual const WaypointList_t& getWaypointList() const { return waypoints; } //!< returns a const reference to #waypoints
00117 
00118   virtual WaypointListIter_t getCurWaypointID() const { return curWaypoint; } //!< returns id value of current waypoint (#curWaypoint)
00119 
00120   virtual float getCurX() const { return curPos[0]; } //!< returns current x position
00121   virtual float getCurY() const { return curPos[1]; } //!< returns current y position
00122   virtual float getCurA() const { return curPos[2]; } //!< returns current heading
00123   //! sets the current position (for instance your localization module has an update)
00124   virtual void setCurPos(float x, float y, float a) {
00125     curPos[0]=x; curPos[1]=y; curPos[2]=a;
00126   }
00127 
00128   //! call this on each opportunity to check current location and correct velocities
00129   /*! @return #isRunning for convenience of checking if anything is happening */
00130   virtual bool cycle(); 
00131 
00132   //!these are for convenience - can also directly edit the waypoint list using access from getWaypointList()
00133   //!@name Adding Waypoints
00134 
00135   //! adds a waypoint to the end of the list, allows you to specify turtle-style instructions
00136   /*! @param forward distance forward to move (negative to move backward of course)
00137    *  @param left distance to the left to move (negative to move right of course)
00138    *  @param angle angle of attack to use on the path
00139    *  @param angleIsRelative controls interpretation of @a angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
00140    *  @param speed is the speed to move at; meters per second */
00141   virtual void addEgocentricWaypoint(float forward, float left, float angle, bool angleIsRelative, float speed) {
00142     waypoints.push_back(Waypoint(forward,left,Waypoint::POSTYPE_EGOCENTRIC,angle,angleIsRelative,speed,isTracking,turnSpeed));
00143   }
00144   //! adds a waypoint to the end of the list, allows you to set locations relative to the location of the previous waypoint (or starting position)
00145   /*! @param x distance delta along x axis of the waypoint
00146    *  @param y distance delta along y axis of the waypoint
00147    *  @param angle angle of attack to use on the path
00148    *  @param angleIsRelative controls interpretation of @a angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
00149    *  @param speed is the speed to move at; meters per second */
00150   virtual void addOffsetWaypoint(float x, float y, float angle, bool angleIsRelative, float speed) {
00151     waypoints.push_back(Waypoint(x,y,Waypoint::POSTYPE_OFFSET,angle,angleIsRelative,speed,isTracking,turnSpeed));
00152   }
00153   //! adds a waypoint to the end of the list, allows you to set locations relative to the location of the previous waypoint (or starting position)
00154   /*! @param x position along x axis of the waypoint
00155    *  @param y position along y axis of the waypoint
00156    *  @param angle angle of attack to use on the path
00157    *  @param angleIsRelative controls interpretation of @a angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
00158    *  @param speed is the speed to move at; meters per second */
00159   virtual void addAbsoluteWaypoint(float x, float y, float angle, bool angleIsRelative, float speed) {
00160     waypoints.push_back(Waypoint(x,y,Waypoint::POSTYPE_ABSOLUTE,angle,angleIsRelative,speed,isTracking,turnSpeed));
00161   }
00162 
00163   //! adds a waypoint to the end of the list, using an arcing path to get there, allows you to specify turtle-style instructions to specify the focus of the arc
00164   /*! If you would rather specify the ending point and then "bow" the path, try addEgocentricWaypoint() followed by setting the Waypoint::arc field directly
00165    *  @param forward distance in front of the center of the circle of the arc
00166    *  @param left distance to the left of the center of the circle of the arc
00167    *  @param angle angle of attack to use on the path
00168    *  @param angleIsRelative controls interpretation of @a angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
00169    *  @param speed is the speed to move at; meters per second
00170    *  @param arc is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability*/
00171   virtual void addEgocentricArc(float forward, float left, float angle, bool angleIsRelative, float speed, float arc) {
00172     addEgocentricWaypoint(forward,left,angle,angleIsRelative,speed);
00173     fixArc(arc);
00174   }
00175   //! adds a waypoint to the end of the list, using an arcing path to get there, allows you to specify locations relative to previous waypoint to specify the focus of the arc
00176   /*! If you would rather specify the ending point and then "bow" the path, try addOffsetWaypoint() followed by setting the Waypoint::arc field directly
00177    *  @param x distance delta along x of the center of the circle of the arc
00178    *  @param y distance delta along y of the center of the circle of the arc
00179    *  @param angle angle of attack to use on the path
00180    *  @param angleIsRelative controls interpretation of @a angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
00181    *  @param speed is the speed to move at; meters per second
00182    *  @param arc is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability*/
00183   virtual void addOffsetArc(float x, float y, float angle, bool angleIsRelative, float speed, float arc) {
00184     addOffsetWaypoint(x,y,angle,angleIsRelative,speed);
00185     fixArc(arc);
00186   }
00187   //! adds a waypoint to the end of the list, using an arcing path to get there, allows you to specify absolute locations to specify the focus of the arc
00188   /*! If you would rather specify the ending point and then "bow" the path, try addAbsoluteWaypoint() followed by setting the Waypoint::arc field directly
00189    *  @param x position along x of the center of the circle of the arc
00190    *  @param y position along y of the center of the circle of the arc
00191    *  @param angle angle of attack to use on the path
00192    *  @param angleIsRelative controls interpretation of @a angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
00193    *  @param speed is the speed to move at; meters per second
00194    *  @param arc is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability*/
00195   virtual void addAbsoluteArc(float x, float y, float angle, bool angleIsRelative, float speed, float arc) {
00196     addAbsoluteWaypoint(x,y,angle,angleIsRelative,speed);
00197     fixArc(arc);
00198   }
00199 
00200   //@}
00201   
00202   //! will set the currently active waypoint to another waypoint; correctly calculates location of intermediate waypoints so target location will be the same as if the intervening waypoints had actually been executed
00203   virtual void setTargetWaypoint(WaypointListIter_t iter) {
00204     //cout << "Moving to waypoint " << iter << endl;
00205     bool isLoop=false;
00206     if(iter==waypoints.end()) {
00207       if(isLooping && waypoints.size()>0) { //restart at beginning
00208         iter=waypoints.begin();
00209         for(unsigned int i=0; i<3; i++)
00210           pathStartPos[i]=curPos[i];
00211         isLoop=true;
00212       } else { //not looping, halt
00213         isRunning=false;
00214         curWaypoint=iter;
00215         for(unsigned int i=0; i<3; i++) {
00216           sourcePos[i]=targetPos[i];
00217           targetPos[i]=curPos[i];
00218           curVel[i]=0;
00219         }       
00220         return;
00221       }
00222     }
00223     if(iter==waypoints.next(curWaypoint) || isLoop)
00224       for(unsigned int i=0; i<3; i++)
00225         sourcePos[i]=targetPos[i];
00226     else
00227       for(unsigned int i=0; i<3; i++)
00228         sourcePos[i]=curPos[i];
00229 
00230     Waypoint target;
00231     if(isLoop)
00232       target=calcAbsoluteCoords(iter,pathStartPos[0],pathStartPos[1],pathStartPos[2]);
00233     else
00234       target=calcAbsoluteCoords(iter);
00235     targetPos[0]=target.x;
00236     targetPos[1]=target.y;
00237     targetPos[2]=target.angle;
00238 
00239     float dx=targetPos[0]-sourcePos[0];
00240     float dy=targetPos[1]-sourcePos[1];
00241     waypointDistance=sqrt(dx*dx+dy*dy);
00242     waypointTime=get_time();
00243     curWaypoint=iter;
00244 
00245     float radiusRatio=sin(waypoints[iter].arc/2);
00246     arcRadius = (radiusRatio==0) ? 0 : (waypointDistance/2)/radiusRatio;
00247     pathLength = arcRadius!=0 ? arcRadius*waypoints[iter].arc : waypointDistance;
00248 
00249     cout << "Target is now: ("<<targetPos[0]<<','<<targetPos[1]<<','<<targetPos[2]<<")" << endl;
00250   }
00251 
00252   //!if @a it follows the current waypoint, applies all the waypoints between #curWaypoint and @a it and returns result as an absolute position (angle field stores heading); otherwise calls the other calcAbsoluteCoords(WaypointListIter_t, float, float, float)
00253   Waypoint calcAbsoluteCoords(WaypointListIter_t it) {
00254     //find out if 'it' is coming up, or already passed
00255     bool isAhead=false;
00256     for(WaypointListIter_t c=curWaypoint; c!=waypoints.end(); c=waypoints.next(c))
00257       if(c==it) {
00258         isAhead=true;
00259         break;
00260       }
00261     if(!isAhead)
00262       return calcAbsoluteCoords(it,pathStartPos[0],pathStartPos[1],pathStartPos[2]);
00263     Waypoint cur(targetPos[0],targetPos[1],Waypoint::POSTYPE_ABSOLUTE,targetPos[2],false,0,isTracking,turnSpeed);
00264     if(it==curWaypoint)
00265       return cur;
00266     for(WaypointListIter_t c=waypoints.next(curWaypoint); c!=waypoints.end(); c=waypoints.next(c)) {
00267       applyWaypoint(cur,waypoints[c]);
00268       if(c==it)
00269         break;
00270     }
00271     return cur;
00272   }
00273 
00274   //!starts at (@a sx, @a sy, heading=@a sa) and then applies all the waypoints up through @a it and returns result as an absolute position (angle field stores heading)
00275   Waypoint calcAbsoluteCoords(WaypointListIter_t it,float sx, float sy, float sa) {
00276     Waypoint cur(sx,sy,Waypoint::POSTYPE_ABSOLUTE,sa,false,0,isTracking,turnSpeed);
00277     for(WaypointListIter_t c=waypoints.begin(); c!=waypoints.end(); c=waypoints.next(c)) {
00278       applyWaypoint(cur,waypoints[c]);
00279       if(c==it)
00280         break;
00281     }
00282     return cur;
00283   }
00284 
00285 
00286 protected:
00287   void init(); //!< basic memory initialization
00288 
00289   //!< if @a next is a relative waypoint (offset or egocentric), it is added to the location held in @a cur; otherwise if @a next is absolute, @a cur is set to @a next
00290   /*! the Waypoint::angle field is used to store the headings */
00291   void applyWaypoint(Waypoint& cur, const Waypoint& next); 
00292 
00293   //! assumes the last waypoint is actually center of circle, adjusts it to be the endpoint of following @a arc radians around that circle instead
00294   void fixArc(float arc);
00295   
00296   //! based on current velocity and time since last call, dead reckons current location in #curPos
00297   /*! doesn't take acceleration into account, but should... :( */
00298   void computeCurrentPosition(unsigned int t);
00299   void checkNextWaypoint(unsigned int t);  //!< checks to see if #curPos is within #eps of #targetPos; if so, setTargetWaypoint() to next waypoint
00300   void computeIdeal(unsigned int t);       //!< computes the ideal location (#idealPos) if we were following the intended path at the intended speed
00301   void computeNewVelocity(unsigned int t); //!< computes the velocity which should be used given the current position (#curPos) relative to the ideal position (#idealPos)
00302 
00303   //! will set a to be between (-pi,pi) (inclusive), just like atan2()
00304   static float normalizeAngle(float a) {
00305     while(a>M_PI)
00306       a-=M_PI*2;
00307     while(a<-M_PI)
00308       a+=M_PI*2;
00309     return a;
00310   }
00311 
00312   //! returns @a x such that it is at most @a max and at minimum @a min
00313   static float clipRange(float x, float min, float max) {
00314     if(x<min)
00315       return min;
00316     else if(x>max)
00317       return max;
00318     else
00319       return x;
00320   }
00321 
00322   WaypointList_t waypoints; //!< storage for the waypoints
00323 
00324   bool isRunning;  //!< true if we're currently executing the path
00325   bool isLooping;  //!< true if we should loop when done
00326   bool isTracking; //!< new waypoints will use trackPath mode
00327   unsigned int curWaypoint;  //!< index of current waypoint
00328   unsigned int waypointTime; //!< time we started working on current waypoint
00329   float waypointDistance;    //!< distance from #sourcePos to #targetPos
00330   float pathLength;          //!< distance to be traveled from #sourcePos to #targetPos (may differ from #waypointDistance due to arcing)
00331   float arcRadius;           //!< radius of current arc, may be inf or NaN if using a straight line; can also be negative depending on direction!
00332   unsigned int lastUpdateTime; //!< time we last updated curPos
00333   float pathStartPos[3]; //!< position when started execution of current path (aka origin offset for relative positions which preceed an absolute waypoint)
00334   float sourcePos[3]; //!< source position of the robot relative to the origin, aka absolute position of previous waypoint
00335   float targetPos[3]; //!< target position of the robot relative to the origin, aka absolute position of next waypoint
00336   float idealPos[4];  //!< ideal position of the robot relative to the origin, (x, y, heading, last element is desired direction of motion)
00337   float curPos[3];    //!< current position of the robot relative to the origin
00338   float curVel[3];    //!< current velocity
00339   float eps[3];       //!< epsilon - "close enough" to register a hit on the waypoint
00340   float Pcorr;        //!< proportional correction factor for tracking path
00341   float turnSpeed;    //!< maximum turning speed for new waypoints
00342 };
00343 
00344 template<unsigned int MAX_WAY>
00345 void WaypointEngine<MAX_WAY>::go() {
00346   isRunning=true;
00347   for(unsigned int i=0; i<3; i++) {
00348     curVel[i]=0;
00349     pathStartPos[i]=sourcePos[i]=curPos[i];
00350   }
00351   Waypoint target(curPos[0],curPos[1],Waypoint::POSTYPE_ABSOLUTE,curPos[2],false,0,isTracking,turnSpeed);
00352   applyWaypoint(target,waypoints.front());
00353   targetPos[0]=target.x;
00354   targetPos[1]=target.y;
00355   targetPos[2]=target.angle;
00356   lastUpdateTime=get_time();
00357   curWaypoint=waypoints.begin();
00358   setTargetWaypoint(curWaypoint);
00359 }
00360 
00361 template<unsigned int MAX_WAY>
00362 void WaypointEngine<MAX_WAY>::pause() {
00363   isRunning=false;
00364 }
00365 
00366 template<unsigned int MAX_WAY>
00367 void WaypointEngine<MAX_WAY>::unpause() {
00368   if(curWaypoint==waypoints.end())
00369     go();
00370   isRunning=true;
00371   for(unsigned int i=0; i<3; i++)
00372     curVel[i]=0;
00373   lastUpdateTime=get_time();
00374 }
00375 
00376 template<unsigned int MAX_WAY>
00377 bool WaypointEngine<MAX_WAY>::cycle() {
00378   if(!isRunning)
00379     return false;
00380   
00381   unsigned int curtime=get_time();
00382   if(curWaypoint!=waypoints.end()) {
00383     computeCurrentPosition(curtime);
00384     checkNextWaypoint(curtime);
00385   }
00386   if(curWaypoint!=waypoints.end()) {
00387     computeIdeal(curtime);
00388     computeNewVelocity(curtime);
00389   }
00390   
00391   return true;
00392 }
00393 
00394 template<unsigned int MAX_WAY>
00395 unsigned int WaypointEngine<MAX_WAY>::getBinSize() const {
00396   unsigned int numPrecision=9;
00397   unsigned int wpSize=0;
00398   unsigned int boilerplateSize=0;
00399   boilerplateSize+=strlen("#WyP\n");
00400   boilerplateSize+=strlen("#add_{point|arc} {ego|off|abs} x_val y_val {hold|follow} angle_val speed_val arc_val\n");
00401   wpSize+=strlen("max_turn_speed ")+numPrecision+1;
00402   wpSize+=strlen("track_path false\n");
00403   wpSize+=strlen("add_point ")+4+numPrecision*5+1*5+strlen("follow");
00404   boilerplateSize+=strlen("#END\n");
00405   return wpSize*waypoints.size()+boilerplateSize;
00406 }
00407 
00408 template<unsigned int MAX_WAY>
00409 unsigned int WaypointEngine<MAX_WAY>::LoadBuffer(const char buf[], unsigned int len) {
00410   unsigned int origlen=len;
00411   waypoints.clear();
00412   if(strncmp("#WyP\n",buf,5)!=0) {
00413     return 0;
00414   }
00415 
00416   float turn=turnSpeed; //init to current mode, in case file doesn't specify
00417   bool track=isTracking;
00418   char cmd[40];
00419   char posType[40];
00420   float x_val=0;
00421   float y_val=0;
00422   char angType[40];
00423   bool ang_val=0;
00424   float angle_val=0;
00425   float speed_val=0;
00426   float arc_val=0;
00427   unsigned int linenum=2;
00428   while(len<=origlen && len>0) {
00429     //    printf("%d %.9s\n",linenum+1,buf);
00430     if(buf[0]=='#' || buf[0]=='\n') {
00431       if(strncmp("#END\n",buf,5)==0)
00432         return origlen-len;
00433       else {
00434         while(*buf++!='\n') {}
00435         continue;
00436       }
00437     }
00438     int used=-1U;
00439     sscanf(buf,"%40s%n",&cmd,&used);
00440     if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint list load corrupted - ran out of room line %d\n",linenum)) return 0;
00441     if(strncasecmp(cmd,"add_point",9)==0 || strncasecmp(cmd,"add_arc",7)==0) {
00442       sscanf(buf,"%40s %g %g %40s %g %g %g%n",posType,&x_val,&y_val,angType,&angle_val,&speed_val,&arc_val,&used);
00443       if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint list load corrupted - bad read on add at line %d\n",linenum)) return 0;
00444       if(strncasecmp(angType,"hold",4)==0)
00445         ang_val=false;
00446       else if(strncasecmp(angType,"follow",6)==0)
00447         ang_val=true;
00448       else {
00449         printf("*** ERROR WaypointEngine: Invalid angle value type %s\n",angType);
00450         return 0;
00451       }
00452       if(strncasecmp(cmd,"add_point",9)==0) {
00453         if(strncasecmp(posType,"ego",3)==0)
00454           addEgocentricWaypoint(x_val,y_val,angle_val,ang_val,speed_val);
00455         else if(strncasecmp(posType,"off",3)==0)
00456           addOffsetWaypoint(x_val,y_val,angle_val,ang_val,speed_val);
00457         else if(strncasecmp(posType,"abs",3)==0)
00458           addAbsoluteWaypoint(x_val,y_val,angle_val,ang_val,speed_val);
00459         else {
00460           printf("*** ERROR WaypointEngine: Invalid position type %s\n",posType);
00461           return 0;
00462         }           
00463         waypoints.back().arc=arc_val;
00464       } else {
00465         if(strncasecmp(posType,"ego",3)==0)
00466           addEgocentricArc(x_val,y_val,angle_val,ang_val,speed_val,arc_val);
00467         else if(strncasecmp(posType,"off",3)==0)
00468           addOffsetArc(x_val,y_val,angle_val,ang_val,speed_val,arc_val);
00469         else if(strncasecmp(posType,"abs",3)==0)
00470           addAbsoluteArc(x_val,y_val,angle_val,ang_val,speed_val,arc_val);
00471         else {
00472           printf("*** ERROR WaypointEngine: Invalid position type %s\n",posType);
00473           return 0;
00474         }
00475       }
00476       waypoints.back().trackPath=track;
00477       waypoints.back().turnSpeed=turn;
00478     } else if(strncasecmp(cmd,"track_path",10)==0) {
00479       sscanf(buf,"%d%n",&track,&used);
00480       if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint load corrupted - bad read on track_path line %d\n",linenum)) return 0;
00481     } else if(strncasecmp(cmd,"max_turn_speed",14)==0) {
00482       sscanf(buf,"%g%n",&turn,&used);
00483       if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint load corrupted - bad read on max_turn_speed line %d\n",linenum)) return 0;
00484     } else {
00485       printf("*** ERROR WaypointEngine: Invalid command %s\n",cmd);
00486       return 0;
00487     }
00488         
00489     linenum++;
00490   }
00491   std::cout << "*** WARNING WaypointEngine: load missing #END" << std::endl;
00492   return origlen-len;
00493 }
00494 
00495 template<unsigned int MAX_WAY>
00496 unsigned int WaypointEngine<MAX_WAY>::SaveBuffer(char buf[], unsigned int len) const {
00497   unsigned int origLen=len;
00498   unsigned int used;
00499   unsigned int cnt=0;
00500     
00501   used=snprintf(buf,len,"#WyP\n");
00502   if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on header\n")) return 0;
00503 
00504   used=snprintf(buf,len,"#add_{point|arc} {ego|off|abs} x_val y_val {hold|follow} angle_val speed_val arc_val\n");
00505   if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on header\n")) return 0;
00506 
00507   //set our state variables so we'll be forced to output the first set
00508   float turn=waypoints.front().turnSpeed-1;
00509   bool track=!waypoints.front().trackPath;
00510 
00511   for(WaypointListIter_t it=waypoints.begin(); it!=waypoints.end(); it=waypoints.next(it)) {
00512     if(waypoints[it].turnSpeed!=turn) {
00513       turn=waypoints[it].turnSpeed;
00514       used=snprintf(buf,len,"max_turn_speed %g\n",turn);
00515       if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on waypoint %d turnSpeed\n",cnt)) return 0;
00516     }
00517     if(waypoints[it].trackPath!=track) {
00518       track=waypoints[it].trackPath;
00519       used=snprintf(buf,len,"track_path %d\n",track);
00520       if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on waypoint %d\n trackPath",cnt)) return 0;
00521     }
00522     const char * posType=NULL;
00523     switch(waypoints[it].posType) {
00524     case Waypoint::POSTYPE_EGOCENTRIC:
00525       posType="EGO"; break;
00526     case Waypoint::POSTYPE_OFFSET:
00527       posType="OFF"; break;
00528     case Waypoint::POSTYPE_ABSOLUTE:
00529       posType="ABS"; break;
00530     }
00531     if(waypoints[it].arc!=0)
00532       used=snprintf(buf,len,"add_point %s %g %g %s %g %g %g\n",posType,waypoints[it].x,waypoints[it].y,(waypoints[it].angleIsRelative?"FOLLOW":"HOLD"),waypoints[it].angle,waypoints[it].speed,waypoints[it].arc);
00533     else //todo - store center of circle
00534       used=snprintf(buf,len,"add_point %s %g %g %s %g %g %g\n",posType,waypoints[it].x,waypoints[it].y,(waypoints[it].angleIsRelative?"FOLLOW":"HOLD"),waypoints[it].angle,waypoints[it].speed,waypoints[it].arc);
00535     if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on waypoint %d\n",cnt)) return 0;
00536     cnt++;
00537   }
00538 
00539   used=snprintf(buf,len,"#END\n");
00540   if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on footer\n")) return 0;
00541 
00542   return origLen-len;
00543 }
00544 
00545 template<unsigned int MAX_WAY>
00546 void WaypointEngine<MAX_WAY>::init() {
00547   eps[0]=eps[1]=.01; //1 centimeter
00548   eps[2]=0.0175; //1 degree
00549   for(unsigned int i=0; i<3; i++)
00550     pathStartPos[i]=targetPos[i]=sourcePos[i]=curPos[i]=curVel[i]=0;
00551   for(unsigned int i=0; i<4; i++)
00552     idealPos[i]=0;
00553 }
00554 
00555 template<unsigned int MAX_WAY>
00556 void WaypointEngine<MAX_WAY>::applyWaypoint(Waypoint& cur, const Waypoint& next) {
00557   float origx=cur.x;
00558   float origy=cur.y;
00559   switch(next.posType) {
00560   case Waypoint::POSTYPE_EGOCENTRIC: {
00561     cur.x+=next.x*cos(cur.angle)-next.y*sin(cur.angle);
00562     cur.y+=next.x*sin(cur.angle)+next.y*cos(cur.angle);
00563     break;
00564   }
00565   case Waypoint::POSTYPE_OFFSET:
00566     cur.x+=next.x;
00567     cur.y+=next.y;
00568     break;
00569   case Waypoint::POSTYPE_ABSOLUTE:
00570     cur.x=next.x;
00571     cur.y=next.y;
00572     break;
00573   }
00574   float dx=cur.x-origx;
00575   float dy=cur.y-origy;
00576   if(fabs(dx)<eps[0] && fabs(dy)<eps[1]) { //turn in place
00577     if(next.angleIsRelative)
00578       cur.angle+=next.angle;
00579     else
00580       cur.angle=next.angle;
00581   } else { //move at heading
00582     cur.angle=next.angle;
00583     if(next.angleIsRelative)
00584       cur.angle+=atan2(dy,dx);
00585   }
00586   cur.angle+=next.arc/2;
00587   cur.angle=normalizeAngle(cur.angle);
00588 }
00589 
00590 template<unsigned int MAX_WAY>
00591 void WaypointEngine<MAX_WAY>::fixArc(float arc) {
00592   Waypoint& center=waypoints.back();
00593   float cdx=center.x; //center delta
00594   float cdy=center.y; //center delta
00595   if(center.posType==Waypoint::POSTYPE_ABSOLUTE) {
00596     //have to find location of waypoint before last one
00597     WaypointListIter_t start_it=waypoints.prev(waypoints.prev(waypoints.end()));
00598     if(start_it!=waypoints.end()) {
00599       Waypoint start=calcAbsoluteCoords(waypoints.prev(waypoints.prev(waypoints.end())));
00600       cdx-=start.x;
00601       cdy-=start.y;
00602     }
00603   }
00604   float r=sqrt(cdx*cdx+cdy*cdy); //radius of circle
00605   float ca=atan2(cdy,cdx); //angle to center of circle
00606   center.x-=r*cos(ca-arc); //now x is the endpoint
00607   center.y-=r*sin(ca-arc); //now y is the endpoint
00608   center.arc=arc;
00609 }
00610   
00611 template<unsigned int MAX_WAY>
00612 void WaypointEngine<MAX_WAY>::computeCurrentPosition(unsigned int t) {
00613   float dt=(t-lastUpdateTime)/1000.f;
00614   float df=dt*curVel[0];
00615   float ds=dt*curVel[1];
00616   float da=dt*curVel[2];
00617 
00618   float avgAngle=curPos[2]+da/2;
00619   float ca=cos(avgAngle);
00620   float sa=sin(avgAngle);
00621 
00622   curPos[0]+=df*ca-ds*sa;
00623   curPos[1]+=df*sa+ds*ca;
00624   curPos[2]+=da;
00625   curPos[2]=normalizeAngle(curPos[2]);
00626 
00627   lastUpdateTime=t;
00628 }
00629 
00630 template<unsigned int MAX_WAY>
00631 void WaypointEngine<MAX_WAY>::checkNextWaypoint(unsigned int /*t*/) {
00632   float rx=targetPos[0]-curPos[0];
00633   float ry=targetPos[1]-curPos[1];
00634   float ra=targetPos[2]-curPos[2];
00635   if(fabs(rx)<eps[0] && fabs(ry)<eps[1] && fabs(ra)<eps[2]) {
00636     //cout << "Check succeeds: " << rx << ' '<<ry<<' ' << ra << endl;
00637     setTargetWaypoint(waypoints.next(curWaypoint));
00638   }
00639 }
00640 
00641 template<unsigned int MAX_WAY>
00642 void WaypointEngine<MAX_WAY>::computeIdeal(unsigned int t) {
00643   Waypoint& cur=waypoints[curWaypoint];
00644   if(cur.trackPath) {
00645     float dx=targetPos[0]-sourcePos[0];
00646     float dy=targetPos[1]-sourcePos[1];
00647     float dt=(t-waypointTime)/1000.f; //time we've been traveling towards current waypoint
00648     float ideal_travel=dt*cur.speed; //distance we should have covered
00649     float p=1; //will be set to percentage of path length to waypoint we have covered
00650     if(pathLength!=0) {
00651       p=ideal_travel/pathLength;
00652       if(p>1)
00653         p=1;
00654     }
00655     if(arcRadius==0) { //radius is "infinite" - straight line solution
00656       idealPos[0]=sourcePos[0]+dx*p;
00657       idealPos[1]=sourcePos[1]+dy*p;
00658       idealPos[2]=targetPos[2];
00659       idealPos[3]=atan2(dy,dx);
00660     } else {
00661       // have to find center of arc's circle
00662       float bearing=atan2(dy,dx); //bearing of target from source
00663       float center_bearing=bearing+(M_PI-cur.arc)/2; //bearing of center from source
00664       float cx=sourcePos[0]+arcRadius*cos(center_bearing); //x pos of center
00665       float cy=sourcePos[1]+arcRadius*sin(center_bearing); //y pos of center
00666       float arc_bearing=center_bearing-M_PI+cur.arc*p; //bearing from center to current ideal location
00667       //sout->printf("%g %g (%g,%g) %g\n",center_bearing,arcRadius,cx,cy,arc_bearing);
00668       idealPos[0]=cx+arcRadius*cos(arc_bearing);
00669       idealPos[1]=cy+arcRadius*sin(arc_bearing);
00670       idealPos[3]=arc_bearing+M_PI/2;
00671       idealPos[2]=cur.angle;
00672       if(cur.angleIsRelative)
00673         idealPos[2]+=idealPos[3];
00674       idealPos[2]=normalizeAngle(idealPos[2]);
00675       idealPos[3]=normalizeAngle(idealPos[3]);
00676     }
00677   } else {
00678     idealPos[0]=curPos[0];
00679     idealPos[1]=curPos[1];
00680     float rx=targetPos[0]-curPos[0];
00681     float ry=targetPos[1]-curPos[1];
00682     if(fabs(rx)<eps[0] && fabs(ry)<eps[1]) {
00683       idealPos[2]=targetPos[2];
00684     } else {
00685       idealPos[2]=cur.angle;
00686       if(cur.angleIsRelative) {
00687         float dx=targetPos[0]-curPos[0];
00688         float dy=targetPos[1]-curPos[1];
00689         idealPos[2]+=atan2(dy,dx);
00690       }
00691       idealPos[2]=normalizeAngle(idealPos[2]);
00692     }
00693     idealPos[3]=atan2(ry,rx);
00694     if(arcRadius!=0) {
00695       float dt=(t-waypointTime)/1000.f; //time we've been traveling towards current waypoint
00696       float ideal_travel=dt*cur.speed; //distance we should have covered
00697       float p=1; //will be set to percentage of path length to waypoint we have covered
00698       if(pathLength!=0) {
00699         p=ideal_travel/pathLength;
00700         if(p>1)
00701           p=1;
00702       }
00703       float arc=cur.arc*(1-p)/2;
00704       idealPos[2]=normalizeAngle(idealPos[2]-arc);
00705       idealPos[3]=normalizeAngle(idealPos[3]-arc);
00706     }
00707   }
00708 }
00709 
00710 template<unsigned int MAX_WAY>
00711 void WaypointEngine<MAX_WAY>::computeNewVelocity(unsigned int /*t*/) {
00712   Waypoint& cur=waypoints[curWaypoint];
00713 
00714   //first we'll start with the velocity we would use if we were on path
00715   //determine distance remaining (only an approximation for arcing paths)
00716   float dx=targetPos[0]-idealPos[0];
00717   float dy=targetPos[1]-idealPos[1];
00718   float spd=sqrt(dx*dx+dy*dy)/(FrameTime*NumFrames)*1000;
00719   if(spd>cur.speed) {
00720     //we're far away - go at full speed
00721     curVel[0]=cur.speed*cos(idealPos[3]-curPos[2]);
00722     curVel[1]=cur.speed*sin(idealPos[3]-curPos[2]);
00723   } else {
00724     //we're about to overshoot... just go fast enough to get on target
00725     curVel[0]=spd*cos(idealPos[3]-curPos[2]);
00726     curVel[1]=spd*sin(idealPos[3]-curPos[2]);
00727   }
00728   if(arcRadius==0)
00729     curVel[2]=0;
00730   else
00731     curVel[2]=cur.speed/arcRadius;
00732 
00733   //sout->printf("ideal vel: %g %g %g\n",curVel[0],curVel[1],curVel[2]);
00734     
00735   // then we'll add the errors
00736   float ex=idealPos[0]-curPos[0];
00737   float ey=idealPos[1]-curPos[1];
00738   float ed=sqrt(ex*ex+ey*ey);
00739   float ehead=atan2(ey,ex)-curPos[2];
00740   float ea=normalizeAngle(idealPos[2]-curPos[2]);
00741   float easpd=ea/(FrameTime*NumFrames)*1000;
00742   easpd=clipRange(easpd,-cur.turnSpeed,cur.turnSpeed);
00743   curVel[0]+=Pcorr*ed*cos(ehead);
00744   curVel[1]+=Pcorr*ed*sin(ehead);
00745   curVel[2]+=easpd;
00746 }
00747 
00748 /*! @file
00749  * @brief Defines WaypointEngine, which provides computation and management of a desired path through a series of waypoints
00750  * @author ejt (Creator)
00751  *
00752  * $Author: ejt $
00753  * $Name: tekkotsu-2_2_1 $
00754  * $Revision: 1.4 $
00755  * $State: Exp $
00756  * $Date: 2004/07/27 14:33:59 $
00757  */
00758 
00759 #endif

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