Tekkotsu: WaypointEngine< MAX_WAY > Class Template Reference

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.

Egocentric: the x and y parameters are relative to the body itself; x is always forward and y is always left. Handy for turtle/logo style specification of instructions

Offset: the x and y parameters are relative to the current body position, but not its heading.

Absolute: the x and y parameters are direct coordinates

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 true or false.

angleIsRelative==true: The angle is relative to the path, so that 0 will keep the robot pointed in the direction of travel, even when arcing. Similarly, $\pi/2\ (90^\circ)$ would cause the robot to walk sideways.

angleIsRelative==false: The angle is relative to the world coordinate system, so a constant heading is maintained throughout execution of the path.

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.

Here you see the results of 3 different arc values $(60^\circ,120^\circ,180^\circ)$ . Note how the arc parameter corresponds to the angle of the circle which is swept.

There are two ways to specify arcs. The add*Waypoint functions use the position arguments to specify the end point of the arc, and the arc parameter serves to "bow" the path. The add*Arc functions specify the center of the circle as the position, and the end point is inferred from the amount of the arc to sweep.

Beware that arcs greater than $180^\circ$ 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 $2\pi\ (360^\circ)$ may cause numerical instability yielding infinitely large circles. Values larger than $2\pi\ (360^\circ)$ will be normalized to the range $[0,2\pi)$ .

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:

The effect of the Waypoint::trackPath flag. When true, the robot will attempt to catch up to its "ideal" location after a perturbation. When false, the robot will ignore the "ideal" path, and just go straight to the destination from wherever perturbations may push it.

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.

Definition at line 116 of file WaypointEngine.h.


Public Types
typedef ListMemBuf< Waypoint, MAX_WAYPOINTS >	WaypointList_t
	convenient shorthand
typedef ListMemBuf< Waypoint, MAX_WAYPOINTS >::index_t	WaypointListIter_t
	convenient shorthand
Public Member Functions
	WaypointEngine ()
	constructor
	WaypointEngine (char *f)
	constructor
virtual unsigned int	getBinSize () const
	returns a rough overestimate of the size needed
virtual unsigned int	loadBuffer (const char buf[], unsigned int len)
	Load from a saved buffer in memory.
virtual unsigned int	saveBuffer (char buf[], unsigned int len) const
	Save to a given buffer in memory.
virtual unsigned int	loadFile (const char *filename)
	initiate opening of the specified file and loading/saving of all appropriate information.
virtual unsigned int	saveFile (const char *filename) const
	initiate opening of the specified file and loading/saving of all appropriate information.
virtual void	go ()
	starts walking towards the first waypoint
virtual void	pause ()
	halts execution of waypoint list
virtual void	unpause ()
	resumes execution of waypoint list from last paused location
virtual void	setIsLooping (bool isl)
	sets isLooping
virtual bool	getIsLooping () const
	returns isLooping
virtual WaypointList_t &	getWaypointList ()
	returns a reference to waypoints
virtual const WaypointList_t &	getWaypointList () const
	returns a const reference to waypoints
virtual WaypointListIter_t	getCurWaypointID () const
	returns id value of current waypoint (curWaypoint)
virtual float	getCurX () const
	returns current x position
virtual float	getCurY () const
	returns current y position
virtual float	getCurA () const
virtual void	setCurPos (float x, float y, float a)
	sets the current position (for instance your localization module has an update)
virtual void	setTracking (bool b)
	sets the isTracking flag, only affects future waypoints which are added, not currently listed waypoints (use getWaypointList() to modify existing waypoints)
virtual bool	getTracking () const
	returns isTracking
virtual bool	cycle ()
	call this on each opportunity to check current location and correct velocities
virtual void	setTargetWaypoint (WaypointListIter_t iter)
	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
Waypoint	calcAbsoluteCoords (WaypointListIter_t it)
	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)
Waypoint	calcAbsoluteCoords (WaypointListIter_t it, float sx, float sy, float sa)
	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)
Adding Waypoints
these are for convenience - can also directly edit the waypoint list using access from getWaypointList()
virtual void	addEgocentricWaypoint (float forward, float left, float angle, bool angleIsRelative, float speed)
	adds a waypoint to the end of the list, allows you to specify turtle-style instructions
virtual void	addOffsetWaypoint (float x, float y, float angle, bool angleIsRelative, float speed)
	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)
virtual void	addAbsoluteWaypoint (float x, float y, float angle, bool angleIsRelative, float speed)
	adds a waypoint to the end of the list, allows you to set locations relative to the world coordinate frame
virtual void	addEgocentricArc (float forward, float left, float angle, bool angleIsRelative, float speed, float arc)
	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
virtual void	addOffsetArc (float x, float y, float angle, bool angleIsRelative, float speed, float arc)
	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
virtual void	addAbsoluteArc (float x, float y, float angle, bool angleIsRelative, float speed, float arc)
	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
Static Public Attributes
static const unsigned int	MAX_WAYPOINTS = MAX_WAY
	for external access to maximum waypoints
Protected Member Functions
void	init ()
	basic memory initialization if next is a relative waypoint (offset or egocentric), it is added to the location held in cur; otherwise if next is absolute, cur is set to next
void	applyWaypoint (Waypoint &cur, const Waypoint &next)
void	fixArc (float arc)
	assumes the last waypoint is actually center of circle, adjusts it to be the endpoint of following arc radians around that circle instead
void	computeCurrentPosition (unsigned int t)
	based on current velocity and time since last call, dead reckons current location in curPos
void	checkNextWaypoint (unsigned int t)
	checks to see if curPos is within eps of targetPos; if so, setTargetWaypoint() to next waypoint
void	computeIdeal (unsigned int t)
	computes the ideal location (idealPos) if we were following the intended path at the intended speed
void	computeNewVelocity (unsigned int t)
	computes the velocity which should be used given the current position (curPos) relative to the ideal position (idealPos)
Static Protected Member Functions
static float	normalizeAngle (float a)
	will set a to be between (-pi,pi) (inclusive), just like atan2()
static float	clipRange (float x, float min, float max)
	returns x such that it is at most max and at minimum min
Protected Attributes
WaypointList_t	waypoints
	storage for the waypoints
bool	isRunning
	true if we're currently executing the path
bool	isLooping
	true if we should loop when done
bool	isTracking
	new waypoints will use trackPath mode
unsigned int	curWaypoint
	index of current waypoint
unsigned int	waypointTime
	time we started working on current waypoint
float	waypointDistance
	distance from sourcePos to targetPos
float	pathLength
	distance to be traveled from sourcePos to targetPos (may differ from waypointDistance due to arcing)
float	arcRadius
	radius of current arc, may be inf or NaN if using a straight line; can also be negative depending on direction!
unsigned int	lastUpdateTime
	time we last updated curPos
float	pathStartPos [3]
	position when started execution of current path (aka origin offset for relative positions which preceed an absolute waypoint)
float	sourcePos [3]
	source position of the robot relative to the origin, aka absolute position of previous waypoint
float	targetPos [3]
	target position of the robot relative to the origin, aka absolute position of next waypoint
float	idealPos [4]
	ideal position of the robot relative to the origin, (x, y, heading, last element is desired direction of motion)
float	curPos [3]
	current position of the robot relative to the origin
float	curVel [3]
	current velocity
float	eps [3]
	epsilon - "close enough" to register a hit on the waypoint
float	Pcorr
	proportional correction factor for tracking path
float	turnSpeed
	maximum turning speed for new waypoints
Classes
struct	Waypoint
	Holds information about each waypoint, see WaypointEngine for overview. More...

Member Function Documentation

template<unsigned int MAX_WAY>

virtual void WaypointEngine< MAX_WAY >::addAbsoluteArc	(	float	x,
		float	y,
		float	angle,
		bool	angleIsRelative,
		float	speed,
		float	arc
	)			`[inline, virtual]`

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

Parameters:

	x	position along x of the center of the circle of the arc
	y	position along y of the center of the circle of the arc
	angle	angle of attack to use on the path
	angleIsRelative	controls interpretation of angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
	speed	is the speed to move at; meters per second
	arc	is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability

Definition at line 266 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::loadBuffer().

template<unsigned int MAX_WAY>

virtual void WaypointEngine< MAX_WAY >::addAbsoluteWaypoint	(	float	x,
		float	y,
		float	angle,
		bool	angleIsRelative,
		float	speed
	)			`[inline, virtual]`

adds a waypoint to the end of the list, allows you to set locations relative to the world coordinate frame

Parameters:

	x	position along x axis of the waypoint
	y	position along y axis of the waypoint
	angle	angle of attack to use on the path
	angleIsRelative	controls interpretation of angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
	speed	is the speed to move at; meters per second

Definition at line 227 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::addAbsoluteArc(), and WaypointEngine< MAX_WAY >::loadBuffer().

template<unsigned int MAX_WAY>

virtual void WaypointEngine< MAX_WAY >::addEgocentricArc	(	float	forward,
		float	left,
		float	angle,
		bool	angleIsRelative,
		float	speed,
		float	arc
	)			`[inline, virtual]`

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

Parameters:

	forward	distance in front of the center of the circle of the arc
	left	distance to the left of the center of the circle of the arc
	angle	angle of attack to use on the path
	angleIsRelative	controls interpretation of angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
	speed	is the speed to move at; meters per second
	arc	is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability

Definition at line 240 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::loadBuffer().

template<unsigned int MAX_WAY>

virtual void WaypointEngine< MAX_WAY >::addEgocentricWaypoint	(	float	forward,
		float	left,
		float	angle,
		bool	angleIsRelative,
		float	speed
	)			`[inline, virtual]`

adds a waypoint to the end of the list, allows you to specify turtle-style instructions

Parameters:

	forward	distance forward to move (negative to move backward of course)
	left	distance to the left to move (negative to move right of course)
	angle	angle of attack to use on the path
	angleIsRelative	controls interpretation of angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
	speed	is the speed to move at; meters per second

Definition at line 207 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::addEgocentricArc(), and WaypointEngine< MAX_WAY >::loadBuffer().

template<unsigned int MAX_WAY>

virtual void WaypointEngine< MAX_WAY >::addOffsetArc	(	float	x,
		float	y,
		float	angle,
		bool	angleIsRelative,
		float	speed,
		float	arc
	)			`[inline, virtual]`

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

Parameters:

	x	distance delta along x of the center of the circle of the arc
	y	distance delta along y of the center of the circle of the arc
	angle	angle of attack to use on the path
	angleIsRelative	controls interpretation of angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
	speed	is the speed to move at; meters per second
	arc	is the number of radians the arc fills; arcs near 0 (or multiples of 360) may cause numeric instability

Definition at line 253 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::loadBuffer().

template<unsigned int MAX_WAY>

virtual void WaypointEngine< MAX_WAY >::addOffsetWaypoint	(	float	x,
		float	y,
		float	angle,
		bool	angleIsRelative,
		float	speed
	)			`[inline, virtual]`

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)

Parameters:

	x	distance delta along x axis of the waypoint
	y	distance delta along y axis of the waypoint
	angle	angle of attack to use on the path
	angleIsRelative	controls interpretation of angle; true means angle specifies an offset from the bearing of the target waypoint, false means maintain an absolute heading
	speed	is the speed to move at; meters per second

Definition at line 217 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::addOffsetArc(), and WaypointEngine< MAX_WAY >::loadBuffer().

template<unsigned int MAX_WAY>

void WaypointEngine< MAX_WAY >::applyWaypoint	(	Waypoint &	cur,
		const Waypoint &	next
	)			`[protected]`

the Waypoint::angle field is used to store the headings

Definition at line 649 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::calcAbsoluteCoords(), and WaypointEngine< MAX_WAY >::go().

template<unsigned int MAX_WAY>

void WaypointEngine< MAX_WAY >::computeCurrentPosition ( unsigned int t ) [protected]

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 705 of file WaypointEngine.h.

Referenced by WaypointEngine< MAX_WAY >::cycle().

template<unsigned int MAX_WAY>

bool WaypointEngine< MAX_WAY >::cycle ( ) [virtual]

call this on each opportunity to check current location and correct velocities

Returns:: isRunning for convenience of checking if anything is happening

Definition at line 448 of file WaypointEngine.h.

Referenced by WaypointWalk< MAX_WAY >::updateOutputs().

template<unsigned int MAX_WAY>

unsigned int WaypointEngine< MAX_WAY >::getBinSize ( ) const [virtual]

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 466 of file WaypointEngine.h.

template<unsigned int MAX_WAY>

virtual float WaypointEngine< MAX_WAY >::getCurA ( ) const [inline, virtual]

returns current heading

Definition at line 184 of file WaypointEngine.h.

template<unsigned int MAX_WAY>

unsigned int WaypointEngine< MAX_WAY >::loadBuffer	(	const char	buf[],
		unsigned int	len
	)			`[virtual]`

Load from a saved buffer in memory.

Parameters:

	buf	pointer to the memory where you should begin loading
	len	length of buf available (this isn't necessarily all yours, there might be other things following your data)

Returns:: the number of bytes actually used

Implements LoadSave.

Definition at line 480 of file WaypointEngine.h.

template<unsigned int MAX_WAY>

virtual unsigned int WaypointEngine< MAX_WAY >::loadFile ( const char * filename ) [inline, virtual]

initiate opening of the specified file and loading/saving of all appropriate information.

Parameters:

filename

the file to load/save

Returns:: number of bytes read/written, 0 if error (or empty)

Reimplemented from LoadSave.

Definition at line 167 of file WaypointEngine.h.

Referenced by WaypointWalk< MAX_WAY >::LoadWaypointFile(), and WaypointEngine< MAX_WAY >::WaypointEngine().

template<unsigned int MAX_WAY>

unsigned int WaypointEngine< MAX_WAY >::saveBuffer	(	char	buf[],
		unsigned int	len
	)			const `[virtual]`

Save to a given buffer in memory.

Parameters:

	buf	pointer to the memory where you should begin writing
	len	length of buf available. (this isn't necessarily all yours, constrain yourself to what you returned in getBinSize() )

Returns:: the number of bytes actually used

Implements LoadSave.

Definition at line 589 of file WaypointEngine.h.

template<unsigned int MAX_WAY>

virtual unsigned int WaypointEngine< MAX_WAY >::saveFile ( const char * filename ) const [inline, virtual]

initiate opening of the specified file and loading/saving of all appropriate information.

Parameters:

filename

the file to load/save

Returns:: number of bytes read/written, 0 if error (or empty)

Reimplemented from LoadSave.

Definition at line 168 of file WaypointEngine.h.

Referenced by WaypointWalk< MAX_WAY >::SaveWaypointFile().

WaypointEngine< MAX_WAY > Class Template Reference

Detailed Description

template<unsigned int MAX_WAY>
class WaypointEngine< MAX_WAY >

Public Types

Public Member Functions

Static Public Attributes

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Classes

Member Function Documentation

WaypointEngine< MAX_WAY > Class Template Reference

Detailed Description

template<unsigned int MAX_WAY> class WaypointEngine< MAX_WAY >

Public Types

Public Member Functions

Static Public Attributes

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Classes

Member Function Documentation

template<unsigned int MAX_WAY>
class WaypointEngine< MAX_WAY >