Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
WaypointEngine Class ReferenceProvides computation and management of a desired path through a series of waypoints. More...
Inheritance diagram for WaypointEngine:
Detailed DescriptionProvides computation and management of a desired path through a series of waypoints. This is a generalized set of data structures and management code - it doesn't actually say anything about how you get from one waypoint to the other, it will just tell you where you should be going at any given time. So, for instance, you may be interested in WaypointWalk, which will use a WalkMC to traverse the waypoints. Future development may include a WaypointPush, to push an object along a path instead of just moving the body along a path. Although general curves between waypoints are not supported, you can use either circular arcs or straight lines. The Waypoint class holds the actual data about each waypoint. You can specify waypoints in 3 ways: egocentric, offset, and absolute.
These specify the position of the next waypoint, but we also need to be able to specify the orientation (heading) of the robot. This is done by specifying an angle and a mode which controls how that angle is interpreted: Waypoint::angleIsRelative, which can be
The final orientation of the robot is simply the heading it was facing when it reaches the end point. To turn in place, you can use a (0,0) egocentric or offset waypoint with an angle parameter. In order to execute curves, you can supply an arc value:
There are two ways to specify arcs. The Beware that arcs greater than are entirely possible, but will create larger and larger circles which may cause the robot to initially start moving away from the destination. This isn't necessarily a bad thing, but may be unanticipated. Values approaching may cause numerical instability yielding infinitely large circles. Values larger than will be normalized to the range . Dead reckoning is very prone to accruing error. It is highly recommended that you calibrate the locomotion mechanism carefully (see WalkCalibration, available under the "Walk Edit" menu with a run-time help menu) and implement some form of localization to handle the inevitable drift. If you have a localization module in place, you can use the setCurPos() function to update the position of the robot within the world. WaypointEngine provides two ways to handle this ensuing discrepency from the path the robot had been tracing:
trackPath is a per-waypoint setting, setTracking() sets the default value for any new waypoints which are thereafter created (the default default is false ;) Waypoint list files are a fairly straightforward plain text format. The extension .wyp is suggested. The waypoint file format is:
Definition at line 119 of file WaypointEngine.h.
Member Typedef Documentation
convenient shorthand Definition at line 122 of file WaypointEngine.h.
convenient shorthand Definition at line 124 of file WaypointEngine.h.
convenient shorthand Definition at line 123 of file WaypointEngine.h. Constructor & Destructor Documentation
constructor Definition at line 127 of file WaypointEngine.h.
constructor Definition at line 133 of file WaypointEngine.h. Member Function Documentation
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 If you would rather specify the ending point and then "bow" the path, try addAbsoluteWaypoint() followed by setting the Waypoint::arc field directly
Definition at line 246 of file WaypointEngine.h. Referenced by loadBuffer().
adds a waypoint to the end of the list, allows you to set locations relative to the world coordinate frame
Definition at line 207 of file WaypointEngine.h. Referenced by addAbsoluteArc(), and loadBuffer().
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 If you would rather specify the ending point and then "bow" the path, try addEgocentricWaypoint() followed by setting the Waypoint::arc field directly
Definition at line 220 of file WaypointEngine.h. Referenced by loadBuffer().
adds a waypoint to the end of the list, allows you to specify turtle-style instructions
Definition at line 89 of file WaypointEngine.cc. Referenced by addEgocentricArc(), and loadBuffer().
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 If you would rather specify the ending point and then "bow" the path, try addOffsetWaypoint() followed by setting the Waypoint::arc field directly
Definition at line 233 of file WaypointEngine.h. Referenced by loadBuffer().
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)
Definition at line 196 of file WaypointEngine.h. Referenced by addOffsetArc(), and loadBuffer().
adds a waypoint to the end of the list, allows you to specify turtle-style instructions
Definition at line 251 of file WaypointEngine.h.
starts at (sx, sy, heading=sa) and then applies all the waypoints up through it and returns result as an absolute position (angle field stores heading) This is replicated in WaypointList, so any updates should be made there as well Definition at line 287 of file WaypointEngine.h.
if it follows the current waypoint, applies all the waypoints between curWaypoint and it and returns result as an absolute position (angle field stores heading); otherwise calls the other calcAbsoluteCoords(WaypointListIter_t, float, float, float) Definition at line 264 of file WaypointEngine.h. Referenced by fixArc(), and setTargetWaypoint().
checks to see if curPos is within eps of targetPos; if so, setTargetWaypoint() to next waypoint Definition at line 364 of file WaypointEngine.cc. Referenced by cycle().
adds a waypoint to the end of the list, allows you to specify turtle-style instructions
Definition at line 255 of file WaypointEngine.h.
based on current velocity and time since last call, dead reckons current location in curPos doesn't take acceleration into account, but should... :( Definition at line 346 of file WaypointEngine.cc. Referenced by cycle().
computes the ideal location (idealPos) if we were following the intended path at the intended speed Definition at line 373 of file WaypointEngine.cc. Referenced by cycle().
computes the velocity which should be used given the current position (curPos) relative to the ideal position (idealPos) Definition at line 443 of file WaypointEngine.cc. Referenced by cycle().
call this on each opportunity to check current location and correct velocities
Definition at line 36 of file WaypointEngine.cc.
assumes the last waypoint is actually center of circle, adjusts it to be the endpoint of following arc radians around that circle instead This is replicated in WaypointList, so any updates should be made there as well Definition at line 324 of file WaypointEngine.cc. Referenced by addAbsoluteArc(), addEgocentricArc(), and addOffsetArc(). Definition at line 81 of file WaypointEngine.cc. Referenced by addEgocentricWaypoint().
returns a rough overestimate of the size needed pretends we need to switch max_turn_speed and track_path on every point, and the longest options are given for every point Implements LoadSave. Definition at line 144 of file WaypointEngine.cc.
returns current heading Definition at line 162 of file WaypointEngine.h.
returns id value of current waypoint (curWaypoint) Definition at line 158 of file WaypointEngine.h.
returns current x position Definition at line 160 of file WaypointEngine.h.
returns current y position Definition at line 161 of file WaypointEngine.h.
returns isLooping Definition at line 153 of file WaypointEngine.h.
returns isTracking Definition at line 169 of file WaypointEngine.h.
returns a const reference to waypoints Definition at line 156 of file WaypointEngine.h.
returns a reference to waypoints Definition at line 155 of file WaypointEngine.h.
starts walking towards the first waypoint Definition at line 4 of file WaypointEngine.cc. Referenced by unpause().
basic memory initialization Definition at line 314 of file WaypointEngine.cc. Referenced by WaypointEngine().
Load from a saved buffer in memory.
Implements LoadSave. Definition at line 157 of file WaypointEngine.cc.
initiate opening of the specified file and loading/saving of all appropriate information.
Reimplemented from LoadSave. Definition at line 145 of file WaypointEngine.h. Referenced by WaypointEngine().
halts execution of waypoint list Definition at line 22 of file WaypointEngine.cc.
Save to a given buffer in memory.
Implements LoadSave. Definition at line 265 of file WaypointEngine.cc.
initiate opening of the specified file and loading/saving of all appropriate information.
Reimplemented from LoadSave. Definition at line 146 of file WaypointEngine.h. sets the current position (for instance your localization module has an update) Definition at line 164 of file WaypointEngine.h.
sets isLooping Definition at line 152 of file WaypointEngine.h.
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 Definition at line 94 of file WaypointEngine.cc. Referenced by checkNextWaypoint(), and go().
sets the isTracking flag, only affects future waypoints which are added, not currently listed waypoints (use getWaypointList() to modify existing waypoints) Definition at line 168 of file WaypointEngine.h. Definition at line 54 of file WaypointEngine.cc. Referenced by fudgedAngle().
resumes execution of waypoint list from last paused location Definition at line 27 of file WaypointEngine.cc. Member Data Documentation
radius of current arc, may be inf or NaN if using a straight line; can also be negative depending on direction! Definition at line 326 of file WaypointEngine.h. Referenced by computeIdeal(), computeNewVelocity(), and setTargetWaypoint().
current position of the robot relative to the origin Definition at line 332 of file WaypointEngine.h. Referenced by checkNextWaypoint(), computeCurrentPosition(), computeIdeal(), computeNewVelocity(), getCurA(), getCurX(), getCurY(), go(), init(), setCurPos(), and setTargetWaypoint().
current velocity Definition at line 333 of file WaypointEngine.h. Referenced by computeCurrentPosition(), computeNewVelocity(), go(), init(), setTargetWaypoint(), and unpause().
index of current waypoint Definition at line 322 of file WaypointEngine.h. Referenced by calcAbsoluteCoords(), checkNextWaypoint(), cycle(), getCurWaypointID(), go(), setTargetWaypoint(), and unpause().
maximum turning speed for new waypoints Definition at line 336 of file WaypointEngine.h. Referenced by addAbsoluteWaypoint(), addEgocentricWaypoint(), addOffsetWaypoint(), calcAbsoluteCoords(), go(), and loadBuffer().
epsilon - "close enough" to register a hit on the waypoint Definition at line 334 of file WaypointEngine.h. Referenced by calcAbsoluteCoords(), checkNextWaypoint(), computeIdeal(), go(), and init().
ideal position of the robot relative to the origin, (x, y, heading, last element is desired direction of motion) Definition at line 331 of file WaypointEngine.h. Referenced by computeIdeal(), computeNewVelocity(), and init().
true if we should loop when done Definition at line 320 of file WaypointEngine.h. Referenced by getIsLooping(), setIsLooping(), and setTargetWaypoint().
true if we're currently executing the path Definition at line 319 of file WaypointEngine.h. Referenced by cycle(), go(), pause(), setTargetWaypoint(), and unpause().
new waypoints will use trackPath mode Definition at line 321 of file WaypointEngine.h. Referenced by addAbsoluteWaypoint(), addEgocentricWaypoint(), addOffsetWaypoint(), calcAbsoluteCoords(), getTracking(), go(), loadBuffer(), and setTracking().
time we last updated curPos Definition at line 327 of file WaypointEngine.h. Referenced by computeCurrentPosition(), go(), and unpause().
distance to be traveled from sourcePos to targetPos (may differ from waypointDistance due to arcing) Definition at line 325 of file WaypointEngine.h. Referenced by computeIdeal(), and setTargetWaypoint().
position when started execution of current path (aka origin offset for relative positions which preceed an absolute waypoint) Definition at line 328 of file WaypointEngine.h. Referenced by calcAbsoluteCoords(), fixArc(), go(), init(), and setTargetWaypoint().
proportional correction factor for tracking path Definition at line 335 of file WaypointEngine.h. Referenced by computeNewVelocity().
source position of the robot relative to the origin, aka absolute position of previous waypoint Definition at line 329 of file WaypointEngine.h. Referenced by computeIdeal(), go(), init(), and setTargetWaypoint().
target position of the robot relative to the origin, aka absolute position of next waypoint Definition at line 330 of file WaypointEngine.h. Referenced by calcAbsoluteCoords(), checkNextWaypoint(), computeIdeal(), computeNewVelocity(), go(), init(), and setTargetWaypoint().
distance from sourcePos to targetPos Definition at line 324 of file WaypointEngine.h. Referenced by setTargetWaypoint().
storage for the waypoints Definition at line 317 of file WaypointEngine.h. Referenced by addAbsoluteWaypoint(), addEgocentricWaypoint(), addOffsetWaypoint(), appendWaypoints(), calcAbsoluteCoords(), clearWaypointList(), cycle(), fixArc(), getBinSize(), getWaypointList(), go(), loadBuffer(), saveBuffer(), setTargetWaypoint(), and unpause().
time we started working on current waypoint Definition at line 323 of file WaypointEngine.h. Referenced by computeIdeal(), and setTargetWaypoint(). The documentation for this class was generated from the following files: |
Tekkotsu v5.1CVS |
Generated Mon May 9 04:59:18 2016 by Doxygen 1.6.3 |