BaseArnl: ArPathPlanningTask Class Reference

The path planning task uses a grid based search to compute the shortest and safe path from the present robot pose to any reachable point in the given robot enviroment map. It then enables an action that follows the planned path, all the while using the dynamic window method to avoid unmapped obstacles in its path. The path planning thread requires fairly accurate robot location. Hence a localization task (ArLocalizationTask or ArSonarLocalizationTask) must also be run concurrently with the path planning task.


Public Types
enum	PathPlanningState { NOT_INITIALIZED, PLANNING_PATH, MOVING_TO_GOAL, REACHED_GOAL, FAILED_PLAN, FAILED_MOVE, ABORTED_PATHPLAN, INVALID }
	State of the path plan (accessible using getState()). More...
enum	LocalPlanningState { NO_LOCAL_PLAN_GFD, NO_LOCAL_PLAN_LFD, NO_LOCAL_PLAN, GOAL_OCCUPIED, NO_MOVE, OBSTACLE_TOO_CLOSE, COMPLETELY_BLOCKED, GOT_PLAN_AND_VEL_HEADING, GOT_PLAN_AND_VELS, GOT_BLOCKED_PLAN_AND_VEL_HEADING, GOT_BLOCKED_PLAN_AND_VELS }
	State of the local motion plan. More...
enum	RangeType { NEITHER, CURRENT, CUMULATIVE, BOTH }
	How to incorporate the range device data. More...
Public Member Functions
	ArPathPlanningTask (ArRobot robot, ArRangeDevice laser, ArRangeDevice sonar, ArMapInterface m)
	Constructor for a robot with two typical range devices (e.g.
	ArPathPlanningTask (ArRobot robot, ArRangeDevice sonar, ArMapInterface *map)
	Constructor for a robot with just sonar (or other comparable range device) and map.
virtual	~ArPathPlanningTask (void)
	Base destructor.
bool	pathPlanToPose (ArPose goal, bool headingFlag, bool printFlag=true)
	Set a new destination pose for the path planning task to plan to.
bool	pathPlanToGoal (const char *goalname, bool strictGoalTypeMatching=true)
	Set a new goal (from the map) for the path planning task to plan to.
bool	startPathPlanToLocalPose (bool printFlag)
	Function to local path planning.
bool	continuePathPlanToLocalPose (ArPose goal, bool headingFlag, bool printFlag=true)
	Set a local pose for the path planning while local path planning.
bool	endPathPlanToLocalPose (bool printFlag)
	Function to end local path planning.
ArActionPlanAndMoveToGoal *	getPathPlanAction (void)
	Gets just the path planning/following action object.
ArActionGroup *	getPathPlanActionGroup (void)
	Gets the path planning action/following group (move, stall recovery etc).
void	setPathPlanActionGroup (ArActionGroup *group, bool takeOwnershipOfGroup=false)
	Sets the path planning action/following group.
bool	loadParamFile (const char *file)
	Load the path planning params from config file.
double	getSafeCollisionRange (void)
	Gets the minimum distance it must look for the speed at which it is going (mostly internal use).
bool	saveParams (char *filename)
	Saves the default params to the filename.
void	addGoalDoneCB (ArFunctor1< ArPose > *functor)
	Adds a callback which will be called when goal reached.
void	remGoalDoneCB (ArFunctor1< ArPose > *functor)
	Removes a callback which used to be called when goal reached.
void	addGoalFailedCB (ArFunctor1< ArPose > *functor)
	Adds a callback which will be called when goal failed.
void	remGoalFailedCB (ArFunctor1< ArPose > *functor)
	Removes a callback which used to be called when goal failed.
void	addGoalInterruptedCB (ArFunctor1< ArPose > *functor)
	Adds a callback which will be called when goal is interrupted.
void	remGoalInterruptedCB (ArFunctor1< ArPose > *functor)
	Removes a callback which used to be called when goal is interrupted.
void	addNewGoalCB (ArFunctor1< ArPose > *functor)
	Adds a callback which will be called when there is a new goal.
void	remNewGoalCB (ArFunctor1< ArPose > *functor)
	Removes a callback which used to be called when there is a new goal.
void	addGoalFinishedCB (ArFunctor *functor)
	Adds a callback for when the goal is done, failed, or intterupted.
void	remGoalFinishedCB (ArFunctor *functor)
	Removes a callback for when the goal is done, failed, or intterupted.
void	addPlainNewGoalCB (ArFunctor *functor, int position=50)
	Adds a plain callback for when there is a new goal.
void	remPlainNewGoalCB (ArFunctor *functor)
	Removes a plain callback for when there is a new goal.
void	addPlainGoalFinishedCB (ArFunctor *functor, int position=50)
	Adds a plain callback for when a goal is finished, interrupted, or failed.
void	remPlainGoalFinishedCB (ArFunctor *functor)
	Removes a plain callback for when a goal is finished, interrupted, or failed.
void	addPlainGoalDoneCB (ArFunctor *functor, int position=50)
	Adds a plain callback for when goal is reached.
void	remPlainGoalDoneCB (ArFunctor *functor)
	Removes a plain callback for when the goal is reached.
void	addPlainGoalFailedCB (ArFunctor *functor, int position=50)
	Adds a plain callback for when the goal fails.
void	remPlainGoalFailedCB (ArFunctor *functor)
	Removes a plain callback for when the goal fails.
void	addPlainGoalInterruptedCB (ArFunctor *functor, int position=50)
	Adds a plain callback for when the goal is interrupted.
void	remPlainGoalInterruptedCB (ArFunctor *functor)
	Removes a plain callback for when the goal is interrupted.
void	trackingFailed (int failedTimes)
	If localization fails, then call this to alert path planning.
void	cancelPathPlan (void)
	Cancel any current path following.
void	setVerboseFlag (bool f)
	Set the verbose flag.
ArPose	getCurrentGoal ()
	Get the current goal of the path plan.
bool	getInitializedFlag (void)
	Get the value of path plan initialized flag.
bool	getVerboseFlag (void)
	Get the verbose flag. (debugging only).
PathPlanningState	getState (void)
	Gets the state of path planning.
void	getFailureString (char *str, size_t len)
	Gets a textual description of failure or current planning status.
void	getStatusString (char *str, size_t len)
	Gets a textual description of failure or current planning status.
std::list< ArPose >	getCurrentPath (ArPose from, bool local=false)
	Gets the list of path points as poses.
ArMapInterface *	getAriaMap (void)
	Gets the Aria map used in the path planning.
void	goalFailed (ArPose goal, const char *failureString="Failed going to goal", PathPlanningState state=FAILED_MOVE)
	What to do when goal cannot be reached.
double	getLastMoveTime (void)
	Get the time robot was not moving.
double	getLastLinearMoveTime (void)
	Get the time robot was not moving.
void	setLastMoveTime (double t)
	Set the time the robot was not moving.
void	setLastLinearMoveTime (double t)
	Set the time the robot was not moving.
double	computeProgress (ArPose robotPose)
	Compute the progress from start to given pose.
double	computeProgress ()
	Compute the progress from start to robot's present pose.
double	estimateTimeToGoal (ArPose robotPose)
	Estimates an approximate time from given pose to goal in seconds..
double	estimateTimeToGoal ()
	Estimates an approximate time from robot's current pose to goal in seconds..
double	estimateDistanceToGoal (ArPose robotPose)
	Estimates an approximate distance from given pose to goal in mm.
double	estimateDistanceToGoal ()
	Estimates an approximate distance from robot's current pose to goal in mm.
ArPathPlan::LocalPathState	getLocalPathState (void)
	Returns the last local path planning result.
ArPathPlan::SearchVelocityState	getDWAState (void)
	Returns the last velocity search state from the DWA.
double	getCost (void)
	Returns cost of the robots cell in the occupancy grid.
double	getUtil (void)
	Returns util of the robots cell in the occupancy grid.
std::list< ArPose >	getPathFromTo (ArPose from, ArPose to)
	Returns just the grid based search path from a point to another.
void	addStateChangeCB (ArFunctor *cb)
	Add a callback to be notified when planning state changes.
void	remStateChangeCB (ArFunctor *cb)
	Remove a callback to be notified when planning state changes.
std::list< ArPose > *	getObsListPtr (void)
	Get the pointer to the list of obstacle points used for path planning.
void	addBlockedPathCB (ArFunctor1< const std::list< ArPose > * > *cb)
	Add a callback function from the list to call when path blocked.
void	remBlockedPathCB (ArFunctor1< const std::list< ArPose > * > *cb)
	Remove a callback function from the list to call when path blocked.
void	addPlainBlockedPathCB (ArFunctor *functor, int position=50)
	Adds a plain callback for when the path is blocked.
void	remPlainBlockedPathCB (ArFunctor *functor)
	Removes a plain callback for when the path is blocked.
void	invokeBlockedPathCB (void)
	Invoke all the callbacks after a replan due to path block.
bool	getWaitingToFailFlag (void)
	Get the waiting to fail flag.
Accessors for current configuration values
These values are normally set via ArConfig (the "Path Planning" section) by loading a file or other means, or by calling the modifier functions below.
bool	getUseLaserFlag (void)
	Get the use laser flag.
bool	getUseSonarFlag (void)
	Get the use sonar flag.
bool	getMapChangedFlag (void)
	Gets the map changed flag to trigger reloading of map for path planning.
bool	getUseOneWaysFlag (void)
	Gets the use one way flag.
bool	getUseResistanceFlag (void)
	Gets the use resistance flag.
double	getMarkOldPathFactor (void)
	Gets the mark old path factor (mostly internal use).
short	getResistance (void)
	Gets the resistance for restrictive sectors (mostly internal use).
double	getMaxSpeed (void) const
	Gets the maximum speed for the robot.
double	getMaxRotSpeed (void) const
	Gets the maximum rotational speed for the robot.
double	getCollisionRange (void) const
	Gets the maximum obstacle range for collision avoidance.
double	getSlowSpeed (void) const
	Gets the slow speed.
double	getFastSpeed (void) const
	Gets the fast speed.
double	getSideClearanceAtSlowSpeed (void) const
	Gets the side clearance at slow speed.
double	getSideClearanceAtFastSpeed (void) const
	Gets the side clearance at fast speed.
double	getFrontClearance (void) const
	Gets the front clearance.
double	getFrontPaddingAtSlowSpeed (void) const
	Gets the front padding at slow speed.
double	getFrontPaddingAtFastSpeed (void) const
	Gets the front padding at high speed.
double	getSuperMaxTransDecel (void) const
	Gets the decel allowed when there is still +ve clearance + padding.
double	getEmergencyMaxTransDecel (void) const
	Gets the emergency max decel allowed for collision avoidance.
double	getWidth (void) const
	Gets the robot width.
double	getLength (void) const
	Gets the robot length.
double	getPlanMapResolution (void) const
	Gets the path planning map resolution.
double	getPlanFreeSpace (void) const
	Gets the distance from the side of the robot to obstacles to plan with.
double	getHeadingWt (void) const
	Gets the heading weight.
double	getDistanceWt (void) const
	Gets the distance weight.
double	getVelocityWt (void) const
	Gets the velocity weight.
double	getSmoothingWt (void) const
	Gets the smoothing weight.
int	getLinVelIncrements (void) const
	Gets the linear velocity increments.
int	getRotVelIncrements (void) const
	Gets the rotational velocity increments.
int	getSmoothSize (void) const
	Gets the smoothing window size.
double	getObsThreshold (void) const
	Gets the obstacle threshold.
double	getMaxExpansionFactor (void) const
	Gets the maximum expansion factor when local planning fails.
bool	getUseCollisionRangeForPlanningFlag (void) const
	Gets the flag which decides if collision range is used for planning.
double	getGoalDistanceTolerance (void) const
	Gets the distance tolerance to goal in mm.
double	getGoalAngleTolerance (void) const
	Gets the angle tolerance to goal in mm.
double	getGoalSpeed (void) const
	Gets the speed for end move.
double	getGoalRotSpeed (void) const
	Gets the rotational speed for end move.
double	getGoalTransAccel (void) const
	Gets the accel for end move.
double	getGoalRotAccel (void) const
	Gets the rotational accel for end move.
double	getGoalTransDecel (void) const
	Gets the decel for end move.
double	getGoalRotDecel (void) const
	Gets the rotational decel for end move.
double	getGoalSwitchTime (void) const
	Gets the time to allow for slowing down.
bool	getGoalUseEncoderFlag (void) const
	Gets the use encoder pose for end move.
double	getGoalOccupiedFailDistance (void) const
	Gets the distance to which the robot will drive when goal is occupied.
double	getLocalPathFailDistance (void) const
	Gets the distance to which the robot will drive when path is blocked.
double	getHeadingRotSpeed (void) const
	Gets the rotational speed for end move.
double	getHeadingRotAccel (void) const
	Gets the rotational accel for end move.
double	getHeadingRotDecel (void) const
	Gets the rotational decel for end move.
int	getSecsToFail (void) const
	Gets the secs for local path search to fail in.
double	getAlignSpeed (void) const
	Gets the max speed at which to go to align mode.
double	getAlignAngle (void) const
	Gets the min angle to local dest to go to align mode.
int	getSplineDegree (void) const
	Gets the degree of the spline.
int	getNumSplinePoints (void) const
	Gets the increments in path to set control points.
bool	getPlanEverytimeFlag (void) const
	Gets the use encoder pose for end move.
bool	getClearOnFailFlag (void) const
	Get the clear on fail flag.
bool	getUseEStopFlag (void) const
	Gets use emergency stop if collision imminent flag.
double	getCurvatureSpeedFactor (void) const
	Gets the curvature speed factor.
double	getOneWayCost (void) const
	Gets the cost of going against the one way direction.
double	getCenterAwayCost (void) const
	Gets the cost of going away from the central spine of the one way area.
double	getResistance (void) const
	Gets the cost of path point inside a resistance area.
double	getUseResistanceFlag (void) const
	Gets the use resistance areas flag.
double	getMarkOldPathFactor (void) const
	Gets the factor by which an existing path point costs less.
bool	getLogFlag (void) const
	Gets the factor by which an existing path point costs less.
int	getLogLevel (void) const
	Gets the currently set logging level use getPathPlanningLogLevel() instead.
double	getMatchLengths (void) const
	Gets the factor to multiply the robot lengths to match path.
bool	getLocalPathPlanningFlag (void) const
	Get the local path planning flag.
Modifiers for configuration values.
Normally these values are automatically set via ArConfig (e.g. by loading them from a file or other source) in the "Path Planning" section, and these methods would only be used internally by ARNL to set stored values from ArConfig.
void	setUseLaserFlag (bool f)
	Sets the use laser flag.
void	setUseSonarFlag (bool f)
	Sets the use sonar flag.
void	setMapChangedFlag (bool f)
	Sets the map changed flag to trigger reloading of map for path planning.
void	setUseOneWaysFlag (bool f)
	Sets the use one way flag.
void	setResistance (short f)
	Sets the reisistance.
void	setPlanDoneCallBack (ArFunctor *prcb)
	Sets the callback which will be called after planning and before action.
void	setUseCollisionRangeForPlanningFlag (bool f)
	Gets the flag which decides if collision range is used for planning.
void	setMaxSpeed (double f)
	Sets the maximum speed during path following.
void	setMaxRotSpeed (double f)
	Sets the maximum rotational speed during path following.
void	setCollisionRange (double f)
	Sets the maximum range for obstacles in sensor view.
void	setSlowSpeed (double f)
	Sets the slow speed for dynamic clearance computation.
void	setFastSpeed (double f)
	Sets the fast speed for dynamic clearance computation.
void	setSideClearanceAtSlowSpeed (double f)
	Sets the side clearance at slow speed.
void	setSideClearanceAtFastSpeed (double f)
	Sets the side clearance at fast speed.
void	setFrontClearance (double f)
	Sets the front clearance.
void	setFrontPaddingAtSlowSpeed (double f)
	Sets the front padding at slow speed.
void	setFrontPaddingAtFastSpeed (double f)
	Sets the front padding at fast speed.
void	setSuperMaxTransDecel (double f)
	Sets the deceleration to be used to avoid obstacles inside padding.
void	setEmergencyMaxTransDecel (double f)
	Set the deceleration to be used when obstacles inside clearance.
void	setWidth (double f)
	Sets the robot width used for path planning.
void	setLength (double f)
	Sets the robot length used for path planning.
void	setPlanMapResolution (double f)
	Sets the grid size for the path planning map.
void	setPlanFreeSpace (double f)
	Sets the distance from obstacles beyond which costs equal free space.
void	setHeadingWt (double f)
	Sets the heading weight for DWA.
void	setDistanceWt (double f)
	Sets the distance weight for DWA.
void	setVelocityWt (double f)
	Sets the velocity weight for DWA.
void	setSmoothingWt (double f)
	Sets the velocity weight for DWA.
void	setLinVelIncrements (int f)
	Sets the linear velocity increments for the DWA table.
void	setRotVelIncrements (int f)
	Sets the rotational velocity increments for the DWA table.
void	setSmoothWinSize (int f)
	Sets the smooth windows size for the DWA.
void	setObsThreshold (double f)
	Sets the threshold above which a cell is considered an obstacle.
void	setMaxExpansionFactor (double f)
	Sets the factor to which the search will expand if it fails.
void	setGoalDistanceTolerance (double f)
	Sets the distance from goal within which end move will kick in.
void	setGoalAngleTolerance (double f)
	Sets the orientation from goal angle within which end move will kick in.
void	setGoalSpeed (double f)
	Sets the speed at which end move will be executed.
void	setGoalRotSpeed (double f)
	Sets the rotational speed at which end move will be executed.
void	setGoalTransAccel (double f)
	Sets the acceleration at which goal will be attempted.
void	setGoalRotAccel (double f)
	Sets the rotational acceleration at which goal will be attempted.
void	setGoalTransDecel (double f)
	Sets the deceleration at which goal will be attempted.
void	setGoalRotDecel (double f)
	Sets the rotational deceleration at which goal will be attempted.
void	setGoalSwitchTime (double f)
	Sets the extra time beyond computed slow down time to do the end move.
void	setGoalUseEncoderFlag (bool f)
	Sets the flag to allow encoder pose for the end move.
void	setGoalOccupiedFailDistance (double f)
	Sets the distance to which the robot will drive when goal is occupied.
void	setHeadingRotSpeed (double f)
	Sets the rotation speed for the Aria heading command.
void	setHeadingRotAccel (double f)
	Sets the rotation accel for the Aria heading command.
void	setHeadingRotDecel (double f)
	Sets the rotation decel for the Aria heading command.
void	setSecsToFail (int f)
	Sets the no of seconds the local planning will keep retrying a fail.
void	setAlignAngle (double f)
	Sets the minimum angle from the path orientation to allow linear motion.
void	setAlignSpeed (double f)
	Sets the minimum speed at which align to path orient will take place.
void	setSplineDegree (int f)
	Sets the degree of the smoothing spline.
void	setNumSplinePoints (int f)
	Sets the distance between knot points on the path to smooth with.
void	setPlanEverytimeFlag (bool f)
	Sets the use encoder pose for end move.
void	setClearOnFailFlag (bool f)
	Set the clear on fail flag.
void	setUseEStopFlag (bool f)
	Sets use emergency stop if collision imminent flag.
void	setUseResistanceFlag (bool f)
	Sets the use resistance areas flag.
void	setMarkOldPathFactor (double f)
	Sets the factor by which an existing path point costs less.
void	setLogFlag (bool f)
	Sets the factor by which an existing path point costs less.
void	setLogLevel (int f)
	Sets the factor by which an existing path point costs less.
void	setMatchLengths (double f)
	Sets the factor to multiply the robot lengths to match path.
void	setLocalPathPlanningFlag (bool f)
	Get the local path planning flag.
void	setPlanParams (double robotWidth, double robotLength, double frontClearance, double sideClearance, double obsThreshold, double maxLinAcc, double maxRotAcc, double maxLinDec, double maxRotDec, double maxLinVel, double maxRotVel, double headingWt, double distanceWt, double velocityWt, double smoothingWt, double gridRes, int nli, int nri, int sws, double maxObsDistance, double goalDistTol, double goalAngTol, double PlanFreeSpace, int secsToFail, double alignAngle, double alignSpeed, int splineDegree, int numSplinePoints, double goalOccupiedFailDistance, double curvatureSpeedFactor, bool useCollisionRangeForPlanningFlag, double oneWayCost, double centerAwayCost, double localPathFailDistance, short resistance, double markOldPathFactor, double matchLengths, bool checkInsideRadius)
	Sets path plan parameters which affect obstacle avoidance & tracking (for internal use).
void	setMovementParams (double linVel, double rotVel, double linAcc, double rotAcc, double linDec, double rotDec)
	Sets only the dynamic motion params (for internal use).
Modify the set of range devices used by ARNL.
void	addRangeDevice (ArRangeDevice *device, RangeType type)
	Adds a range sensing device.
void	remRangeDevice (const char *name, RangeType type)
	Remove a range device from the robot's list, by name.
void	remRangeDevice (ArRangeDevice *device, RangeType type)
	Remove a range device from the robot's list, by instance.
void	addGlobalPlanningRangeDevice (ArRangeDevice *device, RangeType type)
	Adds a global range device.
void	remGlobalPlanningRangeDevice (const char *name, RangeType type)
	Remove a global range device from the robot's list, by name.
void	remGlobalPlanningRangeDevice (ArRangeDevice *device, RangeType type)
	Remove a global range device from the robot's list, by instance.
std::list< ArRangeDevice * > *	getRangeDeviceList (void)
	Gets the range device list.
std::list< ArRangeDevice * > *	getCumRangeDeviceList (void)
	Gets the cumulative range device list.
std::list< ArRangeDevice * > *	getGlobalPlanningRangeDeviceList (void)
	Gets the global range device list.
std::list< ArRangeDevice * > *	getGlobalPlanningCumRangeDeviceList (void)
	Gets the global range device list.
void	clearRangeDevices (void)
	Clears the range devices.
void	clearGlobalPlanningRangeDevices (void)
	Clears the global range devices.
void	clearGlobalPlanningCumRangeDevices (unsigned int cyt)
	Clears the global cum range devices.
void	clearCumRangeDevices (unsigned int cyt)
	Clears the cumulative range devices.
ArNetworking callback methods
Provide ArFunctor objects which invoke these methods to ArServerHandlerDrawings::addDrawing(), and MobileEyes (enable them in the Map menu) and other clients will be able to draw these diagnostic visualizations.
void	drawSearchRectangle (ArServerClient client, ArNetPacket packet)
	Draws the local search rectangle.
void	drawNewPoints (ArServerClient client, ArNetPacket packet)
	Draws the points seen by the sensor and not in the map.
void	drawGridPoints (ArServerClient client, ArNetPacket packet)
	Draws the local special points.
void	drawPathPoints (ArServerClient client, ArNetPacket packet)
	Draws the local path points.
void	drawObsPoints (ArServerClient client, ArNetPacket packet)
	Draws the local obstacle points.
void	drawFailPoints (ArServerClient client, ArNetPacket packet)
	Draws the obs points causing path plan failure.
void	drawVelocityPath (ArServerClient client, ArNetPacket packet)
	Draws the display points.
void	drawRobotBounds (ArServerClient client, ArNetPacket packet)
	Draws the robot boundary.
void	drawCollidePath (ArServerClient client, ArNetPacket packet)
	Draws the collide track.
Protected Member Functions
void	ArPathPlanningTask_CCommon (ArRobot r, ArMapInterface m)
	This contains code common to both constructors above.
virtual void *	runThread (void *ptr)
	Function used to run the task as a thread.
void	robotCallBack (void)
	The sensor interpretation callback. Called every 100msec.
bool	configureLaser (void)
	Needed if the laser does not connect first.
bool	getActionFireFlag (void)
	Get the action fire flag.
bool	getGoalReachedFlag (void)
	Get the goal reached flag.
bool	getSensorSetFlag (void)
	Get the sensor set flag indicating new range data (set every 100ms).
bool	getPlanFailedFlag (void)
	Get the plan failed flag.
bool	getGoalAlteredFlag (void)
	Get the goal altered flag.
bool	getGoalSetFlag (void)
	Get the goal set flag.
bool	getGoalPlannedFlag (void)
	Get the goal planned flag.
bool	getGoalHeadingFlag (void)
	Get the goal heading flag.
bool	getReplanGlobalPathFlag (void)
	Get the path is blocked flag.
unsigned int	getLastPlanTime (void)
	Return time of last replanning.
void	setActionFireFlag (bool f)
	Set the goal set Flag.
void	setCurrentGoal (ArPose p)
	Set the current goal for path planning.
void	setSensorSetFlag (bool f)
	Set the value of flag indicating new sensor data.
void	setInitializedFlag (bool f)
	Set the value of initialized flag.
void	setGoalReachedFlag (bool f)
	Set the goal reached flag.
void	setPlanFailedFlag (bool f)
	Set the plan failed flag.
void	setGoalSetFlag (bool f)
	Set the goal set Flag.
void	setGoalPlannedFlag (bool f)
	Set the goal planned Flag.
void	setGoalHeadingFlag (bool f)
	Set the goal heading Flag.
void	setGoalAlteredFlag (bool f)
	Set the goal heading Flag.
void	setReplanGlobalPathFlag (bool f)
	Set the blocked path flag.
void	setWaitingToFailFlag (bool f)
	Set the waiting to failsecs flag.
void	setLastPlanTime (void)
	Set the time of last plan.
void	setState (PathPlanningState s, const char *failureString=NULL)
	Sets the state of path planning.
void	setProgressNow (double p)
	Sets the progress and time to now.
bool	planAndSetupAction (ArPose from, bool sensorSeesBlock=false)
	Set up the path planning stuff and follow.
LocalPlanningState	computeLocalMotion (ArPose robotPose, ArPose goalPose, double &linVel, double &rotVel, double &heading, double searchFactor, bool replan, double goalDistThres)
	Actually drives the robot along the planned path.
bool	clearMapAndObsList (double cd, ArPose rp)
	Wrapper to get to ArPathPlan to clear its obs point list and map.
void	clearRangeMap (void)
	Clears all transient readings.
bool	incorporateRangeIntoMap (ArRangeDevice *rangeDev, double rangeDist, ArPose robotPose, bool useSensorMap, bool notCumulative)
	Incorporates the sensor readings into the local map for obstacle avoid.
bool	incorporateGlobalPlanningRangeSensors (ArPose rp)
	Incorporate all the global range sensors.
bool	hasGlobalPlanningRangeSensors (void)
	Checks whether there are any global range sensors.
bool	pathEqualsStraightLine (void)
	Checks to see if path to goal is a straight line.
void	setVariableClearances (double lvel, double avel, double &front, double &side)
	Sets the clearances according to speed and return those vals.
void	fillLocalObstacleList (double lvel, double avel, ArPose robotPose)
	Fills the local obstacle list.
double	findDistanceToCollision (double lvel, double avel, ArPose robotPose)
	Find the closest obstacle within collision range.
void	goalDone (ArPose goal)
	What to do when goal is reached.
void	goalInterrupted (ArPose goal)
	What to do when goal was interrupted.
bool	reconfigurePathPlanning (void)
	Needed if the params are changed or loaded again.
void	setupPathPlanningParams (void)
	Setup the path planing params with the aria config thing.
void	mapChanged (void)
	Function for the things to do if map changes.
double	getMaxLinDecel (void)
	Get the linear deceleration.
ArPoseWithTime	getProgress (void)
	Get the progress with time.
void	doSanityCheck (double width, double length, double robotVel, double robotRotVel, double robotRotAccel, double robotRotDecel, double robotAccel, double robotDecel)
	Do a sane check on the param values.

Member Enumeration Documentation

enum ArPathPlanningTask::PathPlanningState

State of the path plan (accessible using getState()).

Enumerator:

NOT_INITIALIZED	Task not initialized.
PLANNING_PATH	Planning the inital path.
MOVING_TO_GOAL	Moving to the goal.
REACHED_GOAL	Reached the goal.
FAILED_PLAN	Failed to plan a path to goal.
FAILED_MOVE	Failed to reach goal after plan obtained.
ABORTED_PATHPLAN	Aborted plan before done.
INVALID	Invalid state.

enum ArPathPlanningTask::LocalPlanningState

State of the local motion plan.

Enumerator:

NO_LOCAL_PLAN_GFD	Failed to plan a path to goal + Goal Fail Dist.
NO_LOCAL_PLAN_LFD	Failed to plan a path to goal + Local Fail Dist.
NO_LOCAL_PLAN	Failed to plan a path to goal.
GOAL_OCCUPIED	Goal is occupied.
NO_MOVE	Failed to reach goal after plan obtained.
OBSTACLE_TOO_CLOSE	Obstacle too close for move.
COMPLETELY_BLOCKED	All directions blocked.
GOT_PLAN_AND_VEL_HEADING	Plan obtained with heading and vel.
GOT_PLAN_AND_VELS	Plan obtained with lin and ang velocities.
GOT_BLOCKED_PLAN_AND_VEL_HEADING	Plan obtained with heading and vel.
GOT_BLOCKED_PLAN_AND_VELS	Plan obtained with lin and ang velocities.

enum ArPathPlanningTask::RangeType

How to incorporate the range device data.

Enumerator:

NEITHER	No current and no cumulative.
CURRENT	Current buffer only.
CUMULATIVE	Cumulative buffer only.
BOTH	Both buffers.

Constructor & Destructor Documentation

ArPathPlanningTask::ArPathPlanningTask	(	ArRobot *	robot,
		ArRangeDevice *	laser,
		ArRangeDevice *	sonar,
		ArMapInterface *	m
	)

Constructor for a robot with two typical range devices (e.g.

SICK LRF and Sonar) and the map. Use addRangeDevice() to add additional range devices. Parameters are set to default values and state is initialized, then the path planning task thread is automatically started.

ArPathPlanningTask::ArPathPlanningTask	(	ArRobot *	robot,
		ArRangeDevice *	sonar,
		ArMapInterface *	map
	)

Constructor for a robot with just sonar (or other comparable range device) and map.

Use addRangeDevice() to add additional range devices. Parameters are set to default values and state is initialized, then the path planning task thread is automatically started.

ArPathPlanningTask::~ArPathPlanningTask ( void ) [virtual]

Base destructor.

Destructor. Kill all newly created classes.

Member Function Documentation

void ArPathPlanningTask::ArPathPlanningTask_CCommon	(	ArRobot *	r,
		ArMapInterface *	m
	)			`[protected]`

This contains code common to both constructors above.

If mySonar or myLaser have been set non-null, then it adds them to the path planning.

bool ArPathPlanningTask::pathPlanToPose	(	ArPose	goal,
		bool	headFlag,
		bool	printFlag = `true`
	)

Set a new destination pose for the path planning task to plan to.

Any current destination goal or pose is interrupted.

This operation is asynchronous: this function returns immediately after updating the destination and other state, and the path planning thread will use the new destination at its next iteration. (You can use the goal success/failure callbacks to recieve notification of the success or failure of the path planning task thread.)

Parameters:

	goal,:	The destination pose.
	headFlag,:	Flag to indicate if robot needs to orient after reaching goal.
	printFlag,:	Flag to log the message.

Returns:: false on as-yet unspecified errors updating destination (none currently), true otherwise

SEEKUR (send this down to unwrap the wheels)

bool ArPathPlanningTask::pathPlanToGoal	(	const char *	goalname,
		bool	strictGoalTypeMatching = `true`
	)

Set a new goal (from the map) for the path planning task to plan to.

Any current goal is interrupted.

Parameters:

	goalname	The destination goal by name.
	strictGoalTypeMatching	If this is true you can only path plan to objects of type Goal and GoalWithHeading, if its false you can path planned to anything named, and if it has WithHeading then it'll use that heading

Returns:: true if the goal exists, false if the goal or map are invalid

bool ArPathPlanningTask::startPathPlanToLocalPose ( bool printFlag )

Function to local path planning.

This is first of the three functions related to wall following.

Parameters:

printFlag,:

Flag to log the message.

Returns:: false on as-yet unspecified errors updating destination (none currently), true otherwise

SEEKUR (send this down to unwrap the wheels)

bool ArPathPlanningTask::continuePathPlanToLocalPose	(	ArPose	goal,
		bool	headFlag,
		bool	printFlag = `true`
	)

Set a local pose for the path planning while local path planning.

Do not use this function if you want to path plan to any point in the map which is not close to the current robot position. Unlike pathPlanToPose, this function does not plan the main path. Instead it makes a straight line path which is then followed up with the regular local path planning and following functions.

Parameters:

	goal,:	The destination pose.
	headFlag,:	Flag to indicate if robot needs to orient after reaching goal.
	printFlag,:	Flag to log the message.

Returns:: false on as-yet unspecified errors updating destination (none currently), true otherwise

bool ArPathPlanningTask::endPathPlanToLocalPose ( bool printFlag )

Function to end local path planning.

This is last of the three functions related to wall following.

Parameters:

printFlag,:

Flag to log the message.

Returns:: false on as-yet unspecified errors updating destination (none currently), true otherwise

void ArPathPlanningTask::setPathPlanActionGroup	(	ArActionGroup *	group,
		bool	takeOwnershipOfGroup = `false`
	)

Sets the path planning action/following group.

This will set the action group the path planning uses. It should still have the action from getPathPlanAction() inside of the group (otherwise path planning won't be able to drive).

Parameters:

	group	the action group to use.
	takeOwnershipOfGroup	if this is true then this object owns the group and will delete it when it is given another one or when this object is deleted.

bool ArPathPlanningTask::loadParamFile ( const char * file )

Load the path planning params from config file.

This is for setting up the parameters from the Aria config file.

Parameters:

file,:

Name of file to load.

Returns:: true if the file could be loaded, false otherwise

double ArPathPlanningTask::getSafeCollisionRange ( void )

Gets the minimum distance it must look for the speed at which it is going (mostly internal use).

Gets the obstacle distance safe for the speed.

Mostly for internal use.

Returns:: : returns the safe obstacle distance.

void ArPathPlanningTask::trackingFailed ( int failedTimes )

If localization fails, then call this to alert path planning.

Response to failed localization tracking, which is to stop the robot.

This is used as a callback from the localization task (ArLocalizationTask or ArSonarLocalizationTask).

Parameters:

failedTimes,:

No of times the tracking failed.

void ArPathPlanningTask::cancelPathPlan ( void )

Cancel any current path following.

Cancel the path planning and move to any goal if any, by resetting state to uninitialized and deactivating the path following action group.

std::list< ArPose > ArPathPlanningTask::getCurrentPath	(	ArPose	from,
		bool	local = `false`
	)

Gets the list of path points as poses.

Gets the Robot path from current to goal as a list of poses.

Returns:: list of poses.

void ArPathPlanningTask::setPlanParams	(	double	robotWidth,
		double	robotLength,
		double	frontClearance,
		double	sideClearance,
		double	obsThreshold,
		double	maxLinAcc,
		double	maxRotAcc,
		double	maxLinDec,
		double	maxRotDec,
		double	maxLinVel,
		double	maxRotVel,
		double	headingWt,
		double	distanceWt,
		double	velocityWt,
		double	smoothingWt,
		double	gridRes,
		int	nli,
		int	nri,
		int	sws,
		double	maxObsDistance,
		double	goalDistTol,
		double	goalAngTol,
		double	planFreeSpace,
		int	secsToFail,
		double	alignAngle,
		double	alignSpeed,
		int	splineDegree,
		int	numSplinePoints,
		double	goalOccupiedFailDistance,
		double	curvatureSpeedFactor,
		bool	useCollisionRangeForPlanningFlag,
		double	oneWayCost,
		double	centerAwayCost,
		double	localPathFailDistance,
		short	resistance,
		double	markOldPathFactor,
		double	matchLengths,
		bool	checkInsideRadius
	)

Sets path plan parameters which affect obstacle avoidance & tracking (for internal use).

For a basic user, change only width, length, front, side, max?acc, max?dec, planres, gdtol and gatol.

Parameters:

	robotWidth,:	Robot Width.
	robotLength,:	Robot Length.
	frontClearance,:	Robot Front Clearance.
	sideClearance,:	Robot Side Clearance.
	obsThreshold,:	Value in the occ grid to be considered obstacle.
	maxLinAcc,:	Maximum Linear acceleration.
	maxRotAcc,:	Maximum Angular acceleration.
	maxLinDec,:	Maximum Linear deceleration.
	maxRotDec,:	Maximum Angular deceleration.
	maxLinVel,:	Maximum Linear velocity.
	maxRotVel,:	Maximum Angular velocity.
	headingWt,:	Heading weight for obs avoidance.
	distanceWt,:	Distance weight for obs avoidance.
	velocityWt,:	Velocity weight for obs avoidance.
	smoothingWt,:	Smoothing weight for obs avoidance.
	gridRes,:	Resolution of the path planning map.
	nli,:	No of linear velocity intervals in the DWA.
	nri,:	No of rotational velocity intervals in the DWA.
	sws,:	Smoothing window size.
	maxObsDistance,:	Maxobsdistureation distance in DWA.
	goalDistTol,:	Goal distacne tolerance to stop move.
	goalAngTol,:	Goal Angle tolerance to stop move.
	headingGradientWt,:	Heading to Gradient weight.
	planFreeSpace,:	Distance beyond this equals free space.
	secsToFail,:	Seconds to keep trying local path search.
	alignAngle,:	Min angle to align before the robot will move.
	alignSpeed,:	Min speed below which robot will align itself.
	splineDegree,:	Degree of the spline curve.
	splineIncrement,:	No of path grid cells to skip when splining.
	goalOccupiedFailDistance,:	Robot will close in this far before failing an occupied goal.
	curvatureSpeedFactor,:	This is the additional factor by which the speed is reduced at turns.
	useCollisionRangeForPlanningFlag,:	Flag to indicate if local path planning can use the collision range for look ahead.
	oneWayCost,:	This is the factor that is used to magnify the penalty of a movement away from the oneway direction for one way planning.
	centerAwayCost,:	This is the factor by which a cell is weighted for oneway planning by the distance it is from the center of the oneway area.
	localPathFailDistance,:	This is the distance the robot will try to still go even when the path is blocked.
	resistance,:	The cost of traversing a cell in a resitrictive ares.
	markOldPathFactor,:	The factor by which old local path cell costs are reduced by.
	matchLengths,:	The multiplier to the robot lengths to set the path matching distance.
	checkInsideRadius,:	Flag to stop movement if obstacles inside robot radius. Can be set to false for legacy.

void ArPathPlanningTask::addRangeDevice	(	ArRangeDevice *	device,
		RangeType	type
	)

Adds a range sensing device.

Adds a range device to use for planning around obstacles.

Parameters:

	name	add a device with this name to the list.
	type,:	Which range buffer to use from the range device.

Examples:: justPathPlanning.cpp.

void ArPathPlanningTask::remRangeDevice	(	const char *	name,
		RangeType	type
	)

Remove a range device from the robot's list, by name.

Removes a range device by name.

Parameters:

	name	remove the first device with this name
	type,:	Type of removal, such as from current, cumulative or both.

void ArPathPlanningTask::remRangeDevice	(	ArRangeDevice *	device,
		RangeType	type
	)

Remove a range device from the robot's list, by instance.

Removes a range device by pointer.

Parameters:

	device,:	Remove the first device with this pointer value
	type,:	Type of removal, such as from current, cumulative or both.

void ArPathPlanningTask::addGlobalPlanningRangeDevice	(	ArRangeDevice *	device,
		RangeType	type
	)

Adds a global range device.

Adds a global range device to use for main path planning.

Parameters:

	name	add a device with this name to the list.
	type,:	Which range buffer to use from the range device.

void ArPathPlanningTask::remGlobalPlanningRangeDevice	(	const char *	name,
		RangeType	type
	)

Remove a global range device from the robot's list, by name.

Removes a global range device by name.

Parameters:

name

remove the first device with this name

void ArPathPlanningTask::remGlobalPlanningRangeDevice	(	ArRangeDevice *	device,
		RangeType	type
	)

Remove a global range device from the robot's list, by instance.

Removes a global range device by pointer.

Parameters:

device,:

Remove the first device with this pointer value

void ArPathPlanningTask::clearRangeDevices ( void )

Clears the range devices.

Clears the buffers of the all range device.

Parameters:

cyt,:

No of milliseconds old data to be killed in cumulative buffer.

void ArPathPlanningTask::clearGlobalPlanningRangeDevices ( void )

Clears the global range devices.

Clears the buffers of the all global range device.

Parameters:

cyt,:

No of milliseconds old data to be killed in cumulative buffer.

void ArPathPlanningTask::clearGlobalPlanningCumRangeDevices ( unsigned int cyt )

Clears the global cum range devices.

Clears the buffers of the all global range device.

Parameters:

cyt,:

No of milliseconds old data to be killed in cumulative buffer.

void ArPathPlanningTask::clearCumRangeDevices ( unsigned int cyt )

Clears the cumulative range devices.

Clears the cumulative buffers of the all range device.

Parameters:

cyt,:

No of milliseconds old data to be killed in cumulative buffer.

void ArPathPlanningTask::goalFailed	(	ArPose	goal,
		const char *	failureString = `"Failed going to goal"`,
		PathPlanningState	state = `FAILED_MOVE`
	)

What to do when goal cannot be reached.

Response to failed path to goal.

Parameters:

goal,:

Goal that was set for the task.

void ArPathPlanningTask::drawPathPoints	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draws the local path points.

ArNetworking callback to send the path drawing as points as opposed to lines

MPL changes so kathleen always gets packet

std::list< ArPose > ArPathPlanningTask::getPathFromTo	(	ArPose	from,
		ArPose	to
	)

Returns just the grid based search path from a point to another.

Compute and return the grid based search path from a point to another. This bare function will not cause the robot to follow the computed path.

Parameters:

	from,:	Pose to compute path from.
	to,:	Pose to compute path to.

Returns:: : A list of poses making up the path.

void ArPathPlanningTask::invokeBlockedPathCB ( void )

Invoke all the callbacks after a replan due to path block.

Call the callbacks to be notified when global range device is used to replan

void * ArPathPlanningTask::runThread ( void * ptr ) [protected, virtual]

Function used to run the task as a thread.

This is the function loop which is called when the path planning task is run as a thread.

Parameters:

ptr,:

Some kind of pointer needed by ARIA.

Returns:: Some kind of pointer decided by ARIA.

void ArPathPlanningTask::setState	(	PathPlanningState	p,
		const char *	statusString = `NULL`
	)			`[protected]`

Sets the state of path planning.

Sets the current state of the path planner.

bool ArPathPlanningTask::planAndSetupAction	(	ArPose	from,
		bool	replan = `false`
	)			`[protected]`

Set up the path planning stuff and follow.

Sets up the path planning and then follow it. Helper for the path planning task thread. Will be set off as soon as a goal is setup.

Parameters:

	from,:	Pose to plan path from. Will be ignored if the current robotPose is further than a cell wide.
	replan,:	Flag indicating global replanning.

Returns:: True or false depending on if a path is found..

MPL and GVP added to try and clear leak that happened when obslist got added to but only cleared in the fire

MPL moved this above the incorporate so that it will always happen (it appears it happens somewhere else first... but just to be sure)

MPL moving this above the incorporate so that it will always happen

MPL changed with george so that the initial path plan would use global replanning devices

ArPathPlanningTask::LocalPlanningState ArPathPlanningTask::computeLocalMotion	(	ArPose	robotPose,
		ArPose	goalPose,
		double &	linVel,
		double &	rotVel,
		double &	heading,
		double	searchFactor,
		bool	replan,
		double	goalDistThres
	)			`[protected]`

Actually drives the robot along the planned path.

Plans a collision free local motion using from the robots current pose and the path to a local goal.

Parameters:

	robotPose,:	Robots current pose. (fed in to avoid locking etc.)
	goalPose,:	Local goal pose to reach.
	linVel,:	Reference to the computed linear velocity.
	rotVel,:	Reference to the computed angular velocity.
	heading,:	Reference to the computed heading.
	searchFactor,:	Factor by which the search window should be expanded.

Returns:: an enum with states of the plan and move.

bool ArPathPlanningTask::incorporateGlobalPlanningRangeSensors ( ArPose robotPose ) [protected]

Incorporate all the global range sensors.

Function to put the appropriate range sensor readings into the sensor map. Will be used by the action when it needs to completely recompute the main path instead of locally modifing it.

bool ArPathPlanningTask::pathEqualsStraightLine ( void ) [protected]

Checks to see if path to goal is a straight line.

Function to check if an encoder based end move is possible because the path to goal is a straight line.

Parameters:

goal,:

Goal pose.

Returns:: True if the path is a straight line.

void ArPathPlanningTask::setVariableClearances	(	double	linVel,
		double	rotVel,
		double &	frontClearance,
		double &	sideClearance
	)			`[protected]`

Sets the clearances according to speed and return those vals.

A wrapper to incorporate the data into the local obstacle list.

Parameters:

	linVel,:	Linear vel of the robot.
	rotVel,:	Angular vel of the robot.

void ArPathPlanningTask::fillLocalObstacleList	(	double	linVel,
		double	rotVel,
		ArPose	robotPose
	)			`[protected]`

Fills the local obstacle list.

A wrapper to incorporate the data into the local obstacle list.

Parameters:

	linVel,:	Linear vel of the robot.
	rotVel,:	Angular vel of the robot.
	robotPose,:	Robot pose at the instant.

double ArPathPlanningTask::findDistanceToCollision	(	double	linVel,
		double	rotVel,
		ArPose	robotPose
	)			`[protected]`

Find the closest obstacle within collision range.

A wrapper to incorporate the cumulative range data into the map.

Parameters:

	linVel,:	Linear vel of the robot.
	rotVel,:	Angular vel of the robot.
	robotPose,:	Robot pose at the instant.

Returns:: true if successful else false.

void ArPathPlanningTask::goalDone ( ArPose goal ) [protected]

What to do when goal is reached.

Response to reaching goal.

Parameters:

goal,:

Goal that was set for the task.

void ArPathPlanningTask::goalInterrupted ( ArPose goal ) [protected]

What to do when goal was interrupted.

Response to interrupted path to goal.

Parameters:

goal,:

Goal that was set for the task.

bool ArPathPlanningTask::reconfigurePathPlanning ( void ) [protected]

Needed if the params are changed or loaded again.

A function to setup the path planning parameters if they are reloaded.

Returns:: 1 if successful else 0.

void ArPathPlanningTask::setupPathPlanningParams ( void ) [protected]

Setup the path planing params with the aria config thing.

Sets all the default parameters for the path planning and sets up the ArConfig struct for handling Aria configuration.

void ArPathPlanningTask::mapChanged ( void ) [protected]

Function for the things to do if map changes.

Things to do if map changes.

void ArPathPlanningTask::doSanityCheck	(	double	width,
		double	length,
		double	robotVel,
		double	robotRotVel,
		double	robotRotAccel,
		double	robotRotDecel,
		double	robotAccel,
		double	robotDecel
	)			`[protected]`

Do a sane check on the param values.

Check if params are sane.

Parameters:

	width,:	Width of the robot.
	length,:	Length of the robot.
	robotVel,:	Reference to the robot vel.
	robotRotVel,:	Reference to the robot rotational vel.
	robotRotAccel,:	Reference to the robot rotational accel.
	robotRotDecel,:	Reference to the robot rotational decel.
	robotAccel,:	Reference to the robot translational accel.
	robotDecel,:	Reference to the robot translational decel.

ArPathPlanningTask Class Reference

Detailed Description

Public Types

Public Member Functions

Protected Member Functions

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation