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 #include "Shared/get_time.h"
00009 #include "WaypointList.h"
00010 #include <cmath>
00011 
00012 //! Provides computation and management of a desired path through a series of waypoints
00013 /*! This is a generalized set of data structures and management code -
00014  *  it doesn't actually say anything about @e how you get from one
00015  *  waypoint to the other, it will just tell you where you should be
00016  *  going at any given time.
00017  *
00018  *  So, for instance, you may be interested in WaypointWalk, which
00019  *  will use a WalkMC to traverse the waypoints.  Future development
00020  *  may include a WaypointPush, to push an object along a path instead
00021  *  of just moving the body along a path.
00022  *
00023  *  Although general curves between waypoints are not supported, you
00024  *  can use either circular arcs or straight lines.
00025  *
00026  *  The Waypoint class holds the actual data about each waypoint.  You
00027  *  can specify waypoints in 3 ways: egocentric, offset, and absolute.
00028  *
00029  *  <table cellspacing=0 cellpadding=0 width="750" class="figures" align="center" border="0"><tr>
00030  *  <td class="figure"><img src="Waypoint_Ego.png"><div style="padding:10px;">
00031  *    @b Egocentric: the x and y parameters are relative to the body
00032  *    itself; x is always forward and y is always left.  Handy for
00033  *    turtle/logo style specification of instructions
00034  *  </div></td><td class="figure"><img src="Waypoint_Off.png"><div style="padding:10px;">
00035  *    @b Offset: the x and y parameters are relative to the current body
00036  *    position, but not its heading.
00037  *  </div></td><td class="figure"><img src="Waypoint_Abs.png"><div style="padding:10px;">
00038  *    @b Absolute: the x and y parameters are direct coordinates
00039  *  </div></td></tr></table>
00040  *
00041  *  These specify the @e position of the next waypoint, but we also need
00042  *  to be able to specify the @e orientation (heading) of the robot.  This
00043  *  is done by specifying an angle and a mode which controls how that
00044  *  angle is interpreted: Waypoint::angleIsRelative, which can be @c true or @c false.
00045  *
00046  *  <table cellspacing=0 cellpadding=0 width="500" class="figures" align="center" border="0"><tr>
00047  *  <td class="figure"><img src="Waypoint_AngleRel.png"><div style="padding:10px;">
00048  *  <code>angleIsRelative==</code>@b true: The angle is relative to the path, so that @c 0 will keep
00049  *  the robot pointed in the direction of travel, even when arcing.  Similarly, @f$\pi/2\ (90^\circ)@f$
00050  *  would cause the robot to walk sideways.
00051  *  </div></td><td class="figure"><img src="Waypoint_AngleAbs.png"><div style="padding:10px;">
00052  *  <code>angleIsRelative==</code>@b false: The angle is relative to the world coordinate system, so a 
00053  *  constant heading is maintained throughout execution of the path.
00054  *  </div></td></tr></table>
00055  *
00056  *  The final orientation of the robot is simply the heading it was
00057  *  facing when it reaches the end point.  To turn in place, you can
00058  *  use a (0,0) egocentric or offset waypoint with an angle parameter.
00059  *
00060  *  In order to execute curves, you can supply an arc value:
00061  *
00062  *  <table cellspacing=0 cellpadding=0 width="417" class="figures" align="center" border="0"><tr>
00063  *  <td class="figure"><img src="Waypoint_Arc.png"><div style="padding:10px;">
00064  *  Here you see the results of 3 different arc values @f$(60^\circ,120^\circ,180^\circ)@f$.  Note how
00065  *  the arc parameter corresponds to the angle of the circle which is swept.<br>
00066  *  </div></td></tr></table>
00067  *
00068  *  There are two ways to specify arcs.  The @c add*Waypoint functions
00069  *  use the position arguments to specify the <em>end point</em> of
00070  *  the arc, and the arc parameter serves to "bow" the path.  The @c
00071  *  add*Arc functions specify the <em>center of the circle</em> as the
00072  *  position, and the end point is inferred from the amount of the arc
00073  *  to sweep.
00074  *
00075  *  Beware that arcs greater than @f$180^\circ@f$ are entirely
00076  *  possible, but will create larger and larger circles which may
00077  *  cause the robot to initially start moving @e away from the
00078  *  destination.  This isn't necessarily a bad thing, but may be
00079  *  unanticipated.  Values approaching @f$2\pi\ (360^\circ)@f$ may
00080  *  cause numerical instability yielding infinitely large circles.
00081  *  Values larger than @f$2\pi\ (360^\circ)@f$ will be normalized to
00082  *  the range @f$[0,2\pi)@f$.
00083  *
00084  *  Dead reckoning is very prone to accruing error.  It is highly
00085  *  recommended that you calibrate the locomotion mechanism carefully
00086  *  (see WalkCalibration, available under the "Walk Edit" menu with a
00087  *  run-time help menu) and implement some form of localization to
00088  *  handle the inevitable drift.
00089  *
00090  *  If you have a localization module in place, you can use the
00091  *  setCurPos() function to update the position of the robot within
00092  *  the world.  WaypointEngine provides two ways to handle this
00093  *  ensuing discrepency from the path the robot had been tracing:
00094  *
00095  *  <table cellspacing=0 cellpadding=0 width="325" class="figures" align="center" border="0"><tr>
00096  *  <td class="figure"><img src="Waypoint_Error.png"><div style="padding:10px;">
00097  *  The effect of the Waypoint::trackPath flag.  When @c true, the robot will attempt to catch up
00098  *  to its "ideal" location after a perturbation.  When @c false, the robot will ignore the "ideal"
00099  *  path, and just go straight to the destination from wherever perturbations may push it.
00100  *  </div></td></tr></table>
00101  *
00102  *  trackPath is a per-waypoint setting, setTracking() sets the
00103  *  default value for any new waypoints which are thereafter created
00104  *  (the default default is false ;)
00105  *
00106  *  Waypoint list files are a fairly straightforward plain text format.
00107  *  The extension .wyp is suggested.
00108  *
00109  *  The waypoint file format is:
00110  *  - '<tt>\#WyP</tt>' - header to verify file type
00111  *  - A series of entries:
00112  *    - '<tt>max_turn_speed </tt><i>num</i>' - sets the maximum error-correction turning speed used for all following waypoints
00113  *    - '<tt>track_path </tt><i>bool</i>' - sets trackpath mode on or off for all following waypoints (see Waypoint::trackPath)
00114  *    - '<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
00115  *    - '<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
00116  *  - '<tt>\#END</tt>' - footer to verify ending of file
00117  */
00118 
00119 class WaypointEngine : public LoadSave {
00120 public:
00121 
00122   typedef std::vector<Waypoint> WaypointList_t; //!< convenient shorthand
00123   typedef std::vector<Waypoint>::iterator WaypointListIter_t; //!< convenient shorthand
00124   typedef std::vector<Waypoint>::const_iterator WaypointListConstIter_t; //!< convenient shorthand
00125 
00126   //! constructor
00127   WaypointEngine()
00128     : LoadSave(), waypoints(), isRunning(false), isLooping(false), isTracking(false),
00129       curWaypoint(waypoints.end()), waypointTime(0), waypointDistance(0), pathLength(0), arcRadius(0),
00130       lastUpdateTime(0), Pcorr(.5f), defaultTurnSpeed((float)M_PI/30)
00131   {init();}
00132   //! constructor
00133   WaypointEngine(char * f)
00134     : LoadSave(), waypoints(), isRunning(false), isLooping(false), isTracking(false),
00135       curWaypoint(waypoints.end()), waypointTime(0), waypointDistance(0), pathLength(0), arcRadius(0),
00136       lastUpdateTime(0), Pcorr(.5f), defaultTurnSpeed((float)M_PI/30)
00137   {init(); loadFile(f); }
00138 
00139   //! returns a rough overestimate of the size needed
00140   /*! pretends we need to switch max_turn_speed and track_path on
00141     every point, and the longest options are given for every point */
00142   virtual unsigned int getBinSize() const;
00143   virtual unsigned int loadBuffer(const char buf[], unsigned int len, const char* filename=NULL);
00144   virtual unsigned int saveBuffer(char buf[], unsigned int len) const;
00145   virtual unsigned int loadFile(const char * filename) { return LoadSave::loadFile(config->motion.makePath(filename).c_str()); }
00146   virtual unsigned int saveFile(const char * filename) const { return LoadSave::saveFile(config->motion.makePath(filename).c_str()); }
00147 
00148   virtual void go();      //!< starts walking towards the first waypoint
00149   virtual void pause();   //!< halts execution of waypoint list
00150   virtual void unpause(); //!< resumes execution of waypoint list from last paused location
00151 
00152   virtual void setIsLooping(bool isl) { isLooping=isl; } //!< sets #isLooping
00153   virtual bool getIsLooping() const { return isLooping; } //!< returns #isLooping
00154 
00155   virtual WaypointList_t& getWaypointList() { return waypoints; } //!< returns a reference to #waypoints
00156   virtual const WaypointList_t& getWaypointList() const { return waypoints; } //!< returns a const reference to #waypoints
00157 
00158   virtual WaypointListIter_t getCurWaypointID() const { return curWaypoint; } //!< returns id value of current waypoint (#curWaypoint)
00159 
00160   virtual float getCurX() const { return curPos[0]; } //!< returns current x position
00161   virtual float getCurY() const { return curPos[1]; } //!< returns current y position
00162   virtual float getCurA() const { return curPos[2]; } //!< returns current heading
00163   //! sets the current position (for instance your localization module has an update)
00164   virtual void setCurPos(float x, float y, float a) {
00165     curPos[0]=x; curPos[1]=y; curPos[2]=a;
00166   }
00167 
00168   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)
00169   virtual bool getTracking() const { return isTracking; } //!< returns #isTracking
00170 
00171   //! call this on each opportunity to check current location and correct velocities
00172   /*! @return #isRunning for convenience of checking if anything is happening */
00173   virtual bool cycle(); 
00174 
00175   //!these are for convenience - can also directly edit the waypoint list using access from getWaypointList()
00176   //!@name Adding Waypoints
00177 
00178   //! adds a waypoint to the end of the list, allows you to specify turtle-style instructions
00179   /*! <img src="Waypoint_Ego.png">
00180    *  @param forward distance forward to move (negative to move backward of course)
00181    *  @param left distance to the left to move (negative to move right of course)
00182    *  @param angle angle of attack to use on the path
00183    *  @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
00184    *  @param fwdSpeed is the speed to move at; millimeters per second
00185          *  @param turnSpeed is the speed to turn; radians per second */
00186   virtual void addEgocentricWaypoint(float forward, float left, float angle, bool angleIsRelative, float fwdSpeed, float turnSpeed=-1.f);
00187 
00188   //! 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)
00189   /*! <img src="Waypoint_Off.png">
00190    *  @param x distance delta along x axis of the waypoint
00191    *  @param y distance delta along y axis of the waypoint
00192    *  @param angle angle of attack to use on the path
00193    *  @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
00194    *  @param fwdSpeed is the speed to move at; millimeters per second
00195          *  @param turnSpeed is the speed to turn; radians per second */
00196   virtual void addOffsetWaypoint(float x, float y, float angle, bool angleIsRelative, float fwdSpeed, float turnSpeed=-1.f) {
00197     waypoints.push_back(Waypoint(x,y,Waypoint::POSTYPE_OFFSET,angle,angleIsRelative,fwdSpeed,isTracking,turnSpeed>=0?turnSpeed:defaultTurnSpeed));
00198   }
00199   //! adds a waypoint to the end of the list, allows you to set locations relative to the world coordinate frame
00200   /*! <img src="Waypoint_Abs.png">
00201    *  @param x position along x axis of the waypoint
00202    *  @param y position along y axis of the waypoint
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 fwdSpeed is the speed to move at; milimeters per second
00206          *  @param turnSpeed is the speed to turn; radians per second */
00207   virtual void addAbsoluteWaypoint(float x, float y, float angle, bool angleIsRelative, float fwdSpeed, float turnSpeed=-1.f) {
00208     waypoints.push_back(Waypoint(x,y,Waypoint::POSTYPE_ABSOLUTE,angle,angleIsRelative,fwdSpeed,isTracking,turnSpeed>=0?turnSpeed:defaultTurnSpeed));
00209   }
00210 
00211   //! 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
00212   /*! <img src="Waypoint_Ego.png">
00213    *  If you would rather specify the ending point and then "bow" the path, try addEgocentricWaypoint() followed by setting the Waypoint::arc field directly
00214    *  @param forward distance in front of the center of the circle of the arc
00215    *  @param left distance to the left of the center of the circle of the arc
00216    *  @param angle angle of attack to use on the path
00217    *  @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
00218    *  @param speed is the speed to move at; millimeters per second
00219    *  @param arc is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability */
00220   virtual void addEgocentricArc(float forward, float left, float angle, bool angleIsRelative, float speed, float arc) {
00221     addEgocentricWaypoint(forward,left,angle,angleIsRelative,speed);
00222     fixArc(arc);
00223   }
00224   //! 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
00225   /*! <img src="Waypoint_Off.png">
00226    *  If you would rather specify the ending point and then "bow" the path, try addOffsetWaypoint() followed by setting the Waypoint::arc field directly
00227    *  @param x distance delta along x of the center of the circle of the arc
00228    *  @param y distance delta along y of the center of the circle of the arc
00229    *  @param angle angle of attack to use on the path
00230    *  @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
00231    *  @param speed is the speed to move at; millimeters per second
00232    *  @param arc is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability */
00233   virtual void addOffsetArc(float x, float y, float angle, bool angleIsRelative, float speed, float arc) {
00234     addOffsetWaypoint(x,y,angle,angleIsRelative,speed);
00235     fixArc(arc);
00236   }
00237   //! 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
00238   /*! <img src="Waypoint_Abs.png">
00239    *  If you would rather specify the ending point and then "bow" the path, try addAbsoluteWaypoint() followed by setting the Waypoint::arc field directly
00240    *  @param x position along x of the center of the circle of the arc
00241    *  @param y position along y of the center of the circle of the arc
00242    *  @param angle angle of attack to use on the path
00243    *  @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
00244    *  @param speed is the speed to move at; millimeters per second
00245    *  @param arc is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability */
00246   virtual void addAbsoluteArc(float x, float y, float angle, bool angleIsRelative, float speed, float arc) {
00247     addAbsoluteWaypoint(x,y,angle,angleIsRelative,speed);
00248     fixArc(arc);
00249   }
00250   
00251   virtual void appendWaypoints(const WaypointList_t wpl) {
00252     waypoints.insert(waypoints.end(), wpl.begin(), wpl.end());
00253   }
00254   
00255   virtual void clearWaypointList() {
00256     waypoints.clear();
00257   }
00258   //@}
00259   
00260   //! 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
00261   virtual void setTargetWaypoint(WaypointListIter_t iter);
00262   
00263   //!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)
00264   Waypoint calcAbsoluteCoords(WaypointListIter_t it) {
00265     //find out if 'it' is coming up, or already passed
00266     bool isAhead=false;
00267     for(WaypointListIter_t c=curWaypoint; c!=waypoints.end(); c++)
00268       if(c==it) {
00269         isAhead=true;
00270         break;
00271       }
00272     if(!isAhead)
00273       return calcAbsoluteCoords(it,pathStartPos[0],pathStartPos[1],pathStartPos[2]);
00274     Waypoint cur(targetPos[0],targetPos[1],Waypoint::POSTYPE_ABSOLUTE,targetPos[2],false,0,isTracking,defaultTurnSpeed);
00275     if(it==curWaypoint)
00276       return cur;
00277     for(WaypointListIter_t c=curWaypoint+1; c!=waypoints.end(); c++) {
00278       cur.apply(*c,eps);
00279       if(c==it)
00280         break;
00281     }
00282     return cur;
00283   }
00284 
00285   //!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)
00286   /*! This is replicated in WaypointList, so any updates should be made there as well */
00287   Waypoint calcAbsoluteCoords(WaypointListIter_t it,float sx, float sy, float sa) {
00288     Waypoint cur(sx,sy,Waypoint::POSTYPE_ABSOLUTE,sa,false,0,isTracking,defaultTurnSpeed);
00289     for(WaypointListIter_t c=waypoints.begin(); c!=waypoints.end(); c++ ) {
00290       cur.apply(*c,eps);
00291       if(c==it)
00292         break;
00293     }
00294     return cur;
00295   }
00296   
00297 
00298 
00299 protected:
00300   void init(); //!< basic memory initialization
00301 
00302   //! assumes the last waypoint is actually center of circle, adjusts it to be the endpoint of following @a arc radians around that circle instead
00303   void fixArc(float arc);
00304   
00305   //! based on current velocity and time since last call, dead reckons current location in #curPos
00306   /*! doesn't take acceleration into account, but should... :( */
00307   void computeCurrentPosition(unsigned int t);
00308   void checkNextWaypoint(unsigned int t);  //!< checks to see if #curPos is within #eps of #targetPos; if so, setTargetWaypoint() to next waypoint
00309   void computeIdeal(unsigned int t);       //!< computes the ideal location (#idealPos) if we were following the intended path at the intended speed
00310   void computeNewVelocity(unsigned int t); //!< computes the velocity which should be used given the current position (#curPos) relative to the ideal position (#idealPos)
00311 
00312 #ifdef TGT_IS_CREATE
00313         static float tgtCreateTurnFudgeFactor(float angle);
00314 #endif
00315         static float fudgedAngle(float originalAngle);
00316 
00317   WaypointList_t waypoints; //!< storage for the waypoints
00318 
00319   bool isRunning;  //!< true if we're currently executing the path
00320   bool isLooping;  //!< true if we should loop when done
00321   bool isTracking; //!< new waypoints will use trackPath mode
00322   WaypointListIter_t curWaypoint;  //!< index of current waypoint
00323   unsigned int waypointTime; //!< time we started working on current waypoint
00324   float waypointDistance;    //!< distance from #sourcePos to #targetPos
00325   float pathLength;          //!< distance to be traveled from #sourcePos to #targetPos (may differ from #waypointDistance due to arcing)
00326   float arcRadius;           //!< radius of current arc, may be inf or NaN if using a straight line; can also be negative depending on direction!
00327   unsigned int lastUpdateTime; //!< time we last updated curPos
00328   float pathStartPos[3]; //!< position when started execution of current path (aka origin offset for relative positions which preceed an absolute waypoint)
00329   float sourcePos[3]; //!< source position of the robot relative to the origin, aka absolute position of previous waypoint
00330   float targetPos[3]; //!< target position of the robot relative to the origin, aka absolute position of next waypoint
00331   float idealPos[4];  //!< ideal position of the robot relative to the origin, (x, y, heading, last element is desired direction of motion)
00332   float curPos[3];    //!< current position of the robot relative to the origin
00333   float curVel[3];    //!< current velocity
00334   float eps[3];       //!< epsilon - "close enough" to register a hit on the waypoint
00335   float Pcorr;        //!< proportional correction factor for tracking path
00336   float defaultTurnSpeed;    //!< maximum turning speed for new waypoints
00337 };
00338 
00339 
00340 /*! @file
00341  * @brief Defines WaypointEngine, which provides computation and management of a desired path through a series of waypoints
00342  * @author ejt (Creator)
00343  */
00344 
00345 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:52 2016 by Doxygen 1.6.3