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

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