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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4