A class implementing a local planner using the Dynamic Window Approach. More...

#include <dwa_planner.h>

Public Member Functions
bool	checkTrajectory (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel)
	Check if a trajectory is legal for a position/velocity pari.
Eigen::Vector3f	computeNewPositions (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
	Given a current position, velocity, and timestep... compute a new position.
base_local_planner::Trajectory	computeTrajectories (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel)
	Given the current position and velocity of the robot, computes and scores a number of possible trajectories to execute, returning the best option.
	DWAPlanner (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
	Constructor for the planner.
base_local_planner::Trajectory	findBestPath (tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > &drive_velocities)
	Given the current position and velocity of the robot, find the best trajectory to exectue.
void	generateTrajectory (Eigen::Vector3f pos, const Eigen::Vector3f &vel, base_local_planner::Trajectory &traj, bool two_point_scoring)
	Given the current position of the robot and a desired velocity, generate and score a trajectory for the given velocity.
Eigen::Vector3f	getAccLimits ()
	Get the acceleration limits of the robot.
bool	getCellCosts (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
	Compute the components and total cost for a map grid cell.
double	getSimPeriod ()
	Get the period at which the local planner is expected to run.
void	updatePlan (const std::vector< geometry_msgs::PoseStamped > &new_plan)
	Take in a new global plan for the local planner to follow.
	~DWAPlanner ()
	Destructor for the planner.
Private Member Functions
double	footprintCost (const Eigen::Vector3f &pos, double scale)
	Given a position for the robot and a scale for the footprint, compute a cost.
Eigen::Vector3f	getMaxSpeedToStopInTime (double time)
	Given a time and the robot's acceleration limits, get the maximum velocity for the robot to be able to stop in the alloted time period.
bool	oscillationCheck (const Eigen::Vector3f &vel)
	For a given velocity, check it it is illegal because of the oscillation flags set.
void	reconfigureCB (DWAPlannerConfig &config, uint32_t level)
	Callback to update the local planner's parameters based on dynamic reconfigure.
void	resetOscillationFlags ()
	Reset the oscillation flags for the local planner.
void	resetOscillationFlagsIfPossible (const Eigen::Vector3f &pos, const Eigen::Vector3f &prev)
	Given the robot's current position and the position where oscillation flags were last set, check to see if the robot had moved far enough to reset them.
void	selectBestTrajectory (base_local_planner::Trajectory &best, base_local_planner::Trajectory &comp)
	Given two trajectories to compare... select the best one.
bool	setOscillationFlags (base_local_planner::Trajectory *t)
	Given a trajectory that's selected, set flags if needed to prevent the robot from oscillating.
double	squareDist (const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
	Compute the square distance between two poses.
Private Attributes
Eigen::Vector3f	acc_lim_
boost::mutex	configuration_mutex_
costmap_2d::Costmap2D	costmap_
costmap_2d::Costmap2DROS *	costmap_ros_
dwa_local_planner::DWAPlannerConfig	default_config_
dynamic_reconfigure::Server < DWAPlannerConfig >	dsrv_
std::vector< geometry_msgs::Point >	footprint_spec_
bool	forward_neg_
bool	forward_neg_only_
double	forward_point_distance_
bool	forward_pos_
bool	forward_pos_only_
base_local_planner::MapGrid	front_map_
double	gdist_scale_
std::vector < geometry_msgs::PoseStamped >	global_plan_
double	heading_lookahead_
double	heading_scale_
base_local_planner::MapGrid	map_
base_local_planner::MapGridVisualizer	map_viz_
	The map grid visualizer for outputting the potential field generated by the cost function.
double	max_scaling_factor_
double	max_vel_th_
double	max_vel_trans_
double	max_vel_x_
double	max_vel_y_
double	min_rot_vel_
double	min_vel_th_
double	min_vel_trans_
double	min_vel_x_
double	min_vel_y_
double	occdist_scale_
double	oscillation_reset_dist_
double	pdist_scale_
bool	penalize_negative_x_
Eigen::Vector3f	prev_stationary_pos_
bool	rot_neg_only_
bool	rot_pos_only_
bool	rotating_neg_
bool	rotating_pos_
double	scaling_speed_
bool	setup_
double	sim_granularity_
double	sim_period_
double	sim_time_
double	stop_time_buffer_
bool	strafe_neg_only_
bool	strafe_pos_only_
bool	strafing_neg_
bool	strafing_pos_
base_local_planner::Trajectory	traj_one_
base_local_planner::Trajectory	traj_two_
Eigen::Vector3f	vsamples_
base_local_planner::CostmapModel *	world_model_

Detailed Description

A class implementing a local planner using the Dynamic Window Approach.

Definition at line 63 of file dwa_planner.h.

Constructor & Destructor Documentation

dwa_local_planner::DWAPlanner::DWAPlanner	(	std::string	name,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

Constructor for the planner.

Parameters:

name	The name of the planner
costmap_ros	A pointer to the costmap instance the planner should use

Definition at line 111 of file dwa_planner.cpp.

dwa_local_planner::DWAPlanner::~DWAPlanner ( ) [inline]

Destructor for the planner.

Definition at line 75 of file dwa_planner.h.

Member Function Documentation

bool dwa_local_planner::DWAPlanner::checkTrajectory	(	const Eigen::Vector3f &	pos,
		const Eigen::Vector3f &	vel
	)

Check if a trajectory is legal for a position/velocity pari.

Parameters:

pos	The robot's position
vel	The desired velocity

Returns:: True if the trajectory is valid, false otherwise

Definition at line 566 of file dwa_planner.cpp.

Eigen::Vector3f dwa_local_planner::DWAPlanner::computeNewPositions	(	const Eigen::Vector3f &	pos,
		const Eigen::Vector3f &	vel,
		double	dt
	)

Given a current position, velocity, and timestep... compute a new position.

Parameters:

pos	The current position
vel	The current velocity
dt	The timestep

Returns:: The new position after applying the velocity for a timestep

Definition at line 180 of file dwa_planner.cpp.

base_local_planner::Trajectory dwa_local_planner::DWAPlanner::computeTrajectories	(	const Eigen::Vector3f &	pos,
		const Eigen::Vector3f &	vel
	)

Given the current position and velocity of the robot, computes and scores a number of possible trajectories to execute, returning the best option.

Parameters:

pos	The current position of the robot
vel	The current velocity of the robot

Returns:: The highest scoring trajectory, a cost >= 0 corresponds to a valid trajectory

Definition at line 234 of file dwa_planner.cpp.

base_local_planner::Trajectory dwa_local_planner::DWAPlanner::findBestPath	(	tf::Stamped< tf::Pose >	global_pose,
		tf::Stamped< tf::Pose >	global_vel,
		tf::Stamped< tf::Pose > &	drive_velocities
	)

Given the current position and velocity of the robot, find the best trajectory to exectue.

Parameters:

global_pose	The current position of the robot
global_vel	The current velocity of the robot
drive_velocities	The velocities to send to the robot base

Returns:: The highest scoring trajectory. A cost >= 0 means the trajectory is legal to execute.

Definition at line 587 of file dwa_planner.cpp.

double dwa_local_planner::DWAPlanner::footprintCost	(	const Eigen::Vector3f &	pos,
		double	scale
	)		`[private]`

Given a position for the robot and a scale for the footprint, compute a cost.

Parameters:

pos	The pose of the robot
scale	The scaling factor for the footprint

Returns:: A cost for the footprint... >= 0 is legal

Definition at line 407 of file dwa_planner.cpp.

void dwa_local_planner::DWAPlanner::generateTrajectory	(	Eigen::Vector3f	pos,
		const Eigen::Vector3f &	vel,
		base_local_planner::Trajectory &	traj,
		bool	two_point_scoring
	)

Given the current position of the robot and a desired velocity, generate and score a trajectory for the given velocity.

Parameters:

pos	The current position of the robot
vel	The desired velocity for the trajectory
traj	A reference to the Trajectory to be populated. A cost >= 0 for the trajectory means that it is valid.
two_point_scoring	Whether to score the trajectory based on the center point of the robot and a point directly in front of the center point, or to score the trajectory only basaed on the center point of the robot

Definition at line 429 of file dwa_planner.cpp.

Eigen::Vector3f dwa_local_planner::DWAPlanner::getAccLimits ( ) [inline]

Get the acceleration limits of the robot.

Returns:: The acceleration limits of the robot

Definition at line 136 of file dwa_planner.h.

bool dwa_local_planner::DWAPlanner::getCellCosts	(	int	cx,
		int	cy,
		float &	path_cost,
		float &	goal_cost,
		float &	occ_cost,
		float &	total_cost
	)

Compute the components and total cost for a map grid cell.

Parameters:

cx	The x coordinate of the cell in the map grid
cy	The y coordinate of the cell in the map grid
path_cost	Will be set to the path distance component of the cost function
goal_cost	Will be set to the goal distance component of the cost function
occ_cost	Will be set to the costmap value of the cell
total_cost	Will be set to the value of the overall cost function, taking into account the scaling parameters

Returns:: True if the cell is traversible and therefore a legal location for the robot to move to

Definition at line 163 of file dwa_planner.cpp.

Eigen::Vector3f dwa_local_planner::DWAPlanner::getMaxSpeedToStopInTime ( double time ) [inline, private]

Given a time and the robot's acceleration limits, get the maximum velocity for the robot to be able to stop in the alloted time period.

Definition at line 213 of file dwa_planner.h.

double dwa_local_planner::DWAPlanner::getSimPeriod ( ) [inline]

Get the period at which the local planner is expected to run.

Returns:: The simulation period

Definition at line 142 of file dwa_planner.h.

bool dwa_local_planner::DWAPlanner::oscillationCheck ( const Eigen::Vector3f & vel ) [private]

For a given velocity, check it it is illegal because of the oscillation flags set.

Definition at line 212 of file dwa_planner.cpp.

void dwa_local_planner::DWAPlanner::reconfigureCB	(	DWAPlannerConfig &	config,
		uint32_t	level
	)		`[private]`

Callback to update the local planner's parameters based on dynamic reconfigure.

Definition at line 41 of file dwa_planner.cpp.

void dwa_local_planner::DWAPlanner::resetOscillationFlags ( ) [private]

Reset the oscillation flags for the local planner.

Definition at line 325 of file dwa_planner.cpp.

void dwa_local_planner::DWAPlanner::resetOscillationFlagsIfPossible	(	const Eigen::Vector3f &	pos,
		const Eigen::Vector3f &	prev
	)		`[private]`

Given the robot's current position and the position where oscillation flags were last set, check to see if the robot had moved far enough to reset them.

Parameters:

pos	The current position of the robot
prev	The position at which the oscillation flags were last set

Returns:

Definition at line 314 of file dwa_planner.cpp.

void dwa_local_planner::DWAPlanner::selectBestTrajectory	(	base_local_planner::Trajectory *&	best,
		base_local_planner::Trajectory *&	comp
	)		`[private]`

Given two trajectories to compare... select the best one.

Parameters:

best	The current best trajectory, will be set to the new best trajectory if comp scores higher
comp	The trajectory to compare to the current best trajectory

Definition at line 189 of file dwa_planner.cpp.

bool dwa_local_planner::DWAPlanner::setOscillationFlags ( base_local_planner::Trajectory * t ) [private]

Given a trajectory that's selected, set flags if needed to prevent the robot from oscillating.

Parameters: