Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world. More...

#include <trajectory_planner.h>

Public Member Functions
bool	checkTrajectory (double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
	Generate and score a single trajectory.
Trajectory	findBestPath (tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > &drive_velocities, geometry_msgs::Twist &ackermann_state)
	Given the current position, orientation, and velocity of the robot, return a trajectory to follow.
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.
void	getLocalGoal (double &x, double &y)
	Accessor for the goal the robot is currently pursuing in world corrdinates.
void	reconfigure (AckermannLocalPlannerConfig &cfg)
	Reconfigures the trajectory planner.
double	scoreTrajectory (double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
	Generate and score a single trajectory.
	TrajectoryPlanner (WorldModel &world_model, const costmap_2d::Costmap2D &costmap, std::vector< geometry_msgs::Point > footprint_spec, double max_acc=1.0, double max_vel=0.3, double min_vel=-0.3, double max_steer_acc=1.0, double max_steer_vel=0.5, double min_steer_vel=-0.5, double max_steer_angle=0.35, double min_steer_angle=-0.35, double axis_distance=1.65, double sim_time=10.0, double sim_granularity=0.025, int vx_samples=20, int vtheta_samples=20, double pdist_scale=0.6, double gdist_scale=0.8, double occdist_scale=0.01, double hdiff_scale=1.0, bool simple_attractor=false, double angular_sim_granularity=0.025, int heading_points=8, double xy_goal_tol=0.5)
	Constructs a trajectory controller.
void	updatePlan (const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
	Update the plan that the controller is following.
	~TrajectoryPlanner ()
	Destructs a trajectory controller.
Private Member Functions
Trajectory	createTrajectories (double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta)
	Create the trajectories we wish to explore, score them, and return the best option.
double	footprintCost (double x_i, double y_i, double theta_i)
	Checks the legality of the robot footprint at a position and orientation using the world model.
void	generateTrajectory (double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, double impossible_cost, Trajectory &traj)
	Generate and score a single trajectory.
void	getFillCells (std::vector< iri_ackermann_local_planner::Position2DInt > &footprint)
	Fill the outline of a polygon, in this case the robot footprint, in a grid.
std::vector < iri_ackermann_local_planner::Position2DInt >	getFootprintCells (double x_i, double y_i, double theta_i, bool fill)
	Used to get the cells that make up the footprint of the robot.
void	getLineCells (int x0, int x1, int y0, int y1, std::vector< iri_ackermann_local_planner::Position2DInt > &pts)
	Use Bresenham's algorithm to trace a line between two points in a grid.
double	headingDiff (int cell_x, int cell_y, double x, double y, double heading)
double	lineCost (int x0, int x1, int y0, int y1)
double	pointCost (int x, int y)
Private Attributes
double	ack_acc_max_
double	ack_axis_distance_
double	ack_steer_acc_max_
double	ack_steer_angle_max_
double	ack_steer_angle_min_
double	ack_steer_speed_max_
double	ack_steer_speed_min_
double	ack_vel_max_
double	ack_vel_min_
double	angular_sim_granularity_
	The distance between angular simulation points.
boost::mutex	configuration_mutex_
const costmap_2d::Costmap2D &	costmap_
	Provides access to cost map information.
std::vector< geometry_msgs::Point >	footprint_spec_
	The footprint specification of the robot.
double	gdist_scale_
std::vector < geometry_msgs::PoseStamped >	global_plan_
	The global path for the robot to follow.
double	goal_x_
double	goal_y_
	Storage for the local goal the robot is pursuing.
double	hdiff_scale_
	Scaling factors for the controller's cost function.
int	heading_points_
MapGrid	map_
	The local map grid where we propagate goal and path distance.
double	occdist_scale_
double	pdist_scale_
double	sim_granularity_
	The distance between simulation points.
double	sim_time_
	The number of seconds each trajectory is "rolled-out".
bool	simple_attractor_
	Enables simple attraction to a goal point.
double	steering_speed_
Trajectory	traj_one
Trajectory	traj_two
	Used for scoring trajectories.
int	vtheta_samples_
	The number of samples we'll take in the theta dimension of the control space.
int	vx_samples_
	The number of samples we'll take in the x dimenstion of the control space.
WorldModel &	world_model_
	The world model that the controller uses for collision detection.
double	xy_goal_tol_
Friends
class	TrajectoryPlannerTest

Detailed Description

Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world.

Definition at line 76 of file trajectory_planner.h.

Constructor & Destructor Documentation

iri_ackermann_local_planner::TrajectoryPlanner::TrajectoryPlanner	(	WorldModel &	world_model,
		const costmap_2d::Costmap2D &	costmap,
		std::vector< geometry_msgs::Point >	footprint_spec,
		double	max_acc = `1.0`,
		double	max_vel = `0.3`,
		double	min_vel = `-0.3`,
		double	max_steer_acc = `1.0`,
		double	max_steer_vel = `0.5`,
		double	min_steer_vel = `-0.5`,
		double	max_steer_angle = `0.35`,
		double	min_steer_angle = `-0.35`,
		double	axis_distance = `1.65`,
		double	sim_time = `10.0`,
		double	sim_granularity = `0.025`,
		int	vx_samples = `20`,
		int	vtheta_samples = `20`,
		double	pdist_scale = `0.6`,
		double	gdist_scale = `0.8`,
		double	occdist_scale = `0.01`,
		double	hdiff_scale = `1.0`,
		bool	simple_attractor = `false`,
		double	angular_sim_granularity = `0.025`,
		int	heading_points = `8`,
		double	xy_goal_tol = `0.5`
	)

Constructs a trajectory controller.

Parameters:

world_model	The WorldModel the trajectory controller uses to check for collisions
costmap	A reference to the Costmap the controller should use
footprint_spec	A polygon representing the footprint of the robot. (Must be convex)
inscribed_radius	The radius of the inscribed circle of the robot
circumscribed_radius	The radius of the circumscribed circle of the robot
acc_lim_x	The acceleration limit of the robot in the x direction
acc_lim_y	The acceleration limit of the robot in the y direction
acc_lim_theta	The acceleration limit of the robot in the theta direction
sim_time	The number of seconds to "roll-out" each trajectory
sim_granularity	The distance between simulation points should be small enough that the robot doesn't hit things
vx_samples	The number of trajectories to sample in the x dimension
vtheta_samples	The number of trajectories to sample in the theta dimension
pdist_scale	A scaling factor for how close the robot should stay to the path
gdist_scale	A scaling factor for how aggresively the robot should pursue a local goal
occdist_scale	A scaling factor for how much the robot should prefer to stay away from obstacles
heading_lookahead	How far the robot should look ahead of itself when differentiating between different rotational velocities
oscillation_reset_dist	The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
escape_reset_dist	The distance the robot must travel before it can exit escape mode
escape_reset_theta	The distance the robot must rotate before it can exit escape mode
holonomic_robot	Set this to true if the robot being controlled can take y velocities and false otherwise
max_vel_x	The maximum x velocity the controller will explore
min_vel_x	The minimum x velocity the controller will explore
max_vel_th	The maximum rotational velocity the controller will explore
min_vel_th	The minimum rotational velocity the controller will explore
min_in_place_vel_th	The absolute value of the minimum in-place rotational velocity the controller will explore
backup_vel	The velocity to use while backing up
dwa	Set this to true to use the Dynamic Window Approach, false to use acceleration limits
heading_scoring	Set this to true to score trajectories based on the robot's heading after 1 timestep
heading_scoring_timestep	How far to look ahead in time when we score heading based trajectories
simple_attractor	Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
y_vels	A vector of the y velocities the controller will explore
angular_sim_granularity	The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things

Definition at line 89 of file trajectory_planner.cpp.

iri_ackermann_local_planner::TrajectoryPlanner::~TrajectoryPlanner ( )

Destructs a trajectory controller.

Definition at line 111 of file trajectory_planner.cpp.

Member Function Documentation

bool iri_ackermann_local_planner::TrajectoryPlanner::checkTrajectory	(	double	x,
		double	y,
		double	theta,
		double	vx,
		double	vy,
		double	vtheta,
		double	vx_samp,
		double	vy_samp,
		double	vtheta_samp
	)

Generate and score a single trajectory.

Parameters:

x	The x position of the robot
y	The y position of the robot
theta	The orientation of the robot
vx	The x velocity of the robot
vy	The y velocity of the robot
vtheta	The theta velocity of the robot
vx_samp	The x velocity used to seed the trajectory
vy_samp	The y velocity used to seed the trajectory
vtheta_samp	The theta velocity used to seed the trajectory

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

Definition at line 684 of file trajectory_planner.cpp.

Trajectory iri_ackermann_local_planner::TrajectoryPlanner::createTrajectories	(	double	x,
		double	y,
		double	theta,
		double	vx,
		double	vy,
		double	vtheta,
		double	acc_x,
		double	acc_y,
		double	acc_theta
	)		`[private]`

Create the trajectories we wish to explore, score them, and return the best option.

Parameters:

x	The x position of the robot
y	The y position of the robot
theta	The orientation of the robot
vx	The x velocity of the robot
vy	The y velocity of the robot
vtheta	The theta velocity of the robot
acc_x	The x acceleration limit of the robot
acc_y	The y acceleration limit of the robot
acc_theta	The theta acceleration limit of the robot

Returns:

Definition at line 710 of file trajectory_planner.cpp.

Trajectory iri_ackermann_local_planner::TrajectoryPlanner::findBestPath	(	tf::Stamped< tf::Pose >	global_pose,
		tf::Stamped< tf::Pose >	global_vel,
		tf::Stamped< tf::Pose > &	drive_velocities,
		geometry_msgs::Twist &	ackermann_state
	)

Given the current position, orientation, and velocity of the robot, return a trajectory to follow.

Parameters:

global_pose	The current pose of the robot in world space
global_vel	The current velocity of the robot in world space
drive_velocities	Will be set to velocities to send to the robot base

Returns:: The selected path or trajectory

Definition at line 984 of file trajectory_planner.cpp.

double iri_ackermann_local_planner::TrajectoryPlanner::footprintCost	(	double	x_i,
		double	y_i,
		double	theta_i
	)		`[private]`

Checks the legality of the robot footprint at a position and orientation using the world model.

Parameters:

x_i	The x position of the robot
y_i	The y position of the robot
theta_i	The orientation of the robot

Returns:

Definition at line 1054 of file trajectory_planner.cpp.

void iri_ackermann_local_planner::TrajectoryPlanner::generateTrajectory	(	double	x,
		double	y,
		double	theta,
		double	vx,
		double	vy,
		double	vtheta,
		double	vx_samp,
		double	vy_samp,
		double	vtheta_samp,
		double	acc_x,
		double	acc_y,
		double	acc_theta,
		double	impossible_cost,
		Trajectory &	traj
	)		`[private]`

Generate and score a single trajectory.

Parameters:

x	The x position of the robot
y	The y position of the robot
theta	The orientation of the robot
vx	The x velocity of the robot
vy	The y velocity of the robot
vtheta	The theta velocity of the robot
vx_samp	The x velocity used to seed the trajectory
vy_samp	The y velocity used to seed the trajectory
vtheta_samp	The theta velocity used to seed the trajectory
acc_x	The x acceleration limit of the robot
acc_y	The y acceleration limit of the robot
acc_theta	The theta acceleration limit of the robot
impossible_cost	The cost value of a cell in the local map grid that is considered impassable
traj	Will be set to the generated trajectory with its associated score

Definition at line 129 of file trajectory_planner.cpp.

bool iri_ackermann_local_planner::TrajectoryPlanner::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 113 of file trajectory_planner.cpp.

void iri_ackermann_local_planner::TrajectoryPlanner::getFillCells ( std::vector< iri_ackermann_local_planner::Position2DInt > & footprint ) [private]

Fill the outline of a polygon, in this case the robot footprint, in a grid.

Parameters:

footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint

vector< iri_ackermann_local_planner::Position2DInt > iri_ackermann_local_planner::TrajectoryPlanner::getFootprintCells	(	double	x_i,
		double	y_i,
		double	theta_i,
		bool	fill
	)		`[private]`

Used to get the cells that make up the footprint of the robot.

Parameters:

x_i	The x position of the robot
y_i	The y position of the robot
theta_i	The orientation of the robot
fill	If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.

Returns:: The cells that make up either the outline or entire footprint of the robot depending on fill

Definition at line 1148 of file trajectory_planner.cpp.

void iri_ackermann_local_planner::TrajectoryPlanner::getLineCells	(	int	x0,
		int	x1,
		int	y0,
		int	y1,
		std::vector< iri_ackermann_local_planner::Position2DInt > &	pts
	)		`[private]`

Use Bresenham's algorithm to trace a line between two points in a grid.

Parameters:

x0	The x coordinate of the first point
x1	The x coordinate of the second point
y0	The y coordinate of the first point
y1	The y coordinate of the second point
pts	Will be filled with the cells that lie on the line in the grid

void iri_ackermann_local_planner::TrajectoryPlanner::getLocalGoal	(	double &	x,
		double &	y
	)

Accessor for the goal the robot is currently pursuing in world corrdinates.

Parameters:

x	Will be set to the x position of the local goal
y	Will be set to the y position of the local goal

Definition at line 1258 of file trajectory_planner.cpp.

double iri_ackermann_local_planner::TrajectoryPlanner::headingDiff	(	int	cell_x,
		int	cell_y,
		double	x,
		double	y,
		double	heading
	)		`[private]`

Definition at line 549 of file trajectory_planner.cpp.

double iri_ackermann_local_planner::TrajectoryPlanner::lineCost	(	int	x0,
		int	x1,
		int	y0,
		int	y1
	)		`[private]`

Definition at line 579 of file trajectory_planner.cpp.

double iri_ackermann_local_planner::TrajectoryPlanner::pointCost	(	int	x,
		int	y
	)		`[private]`

Definition at line 658 of file trajectory_planner.cpp.

void iri_ackermann_local_planner::TrajectoryPlanner::reconfigure ( AckermannLocalPlannerConfig & cfg )

Reconfigures the trajectory planner.

Definition at line 43 of file trajectory_planner.cpp.

double iri_ackermann_local_planner::TrajectoryPlanner::scoreTrajectory	(	double	x,
		double	y,
		double	theta,
		double	vx,
		double	vy,
		double	vtheta,
		double	vx_samp,
		double	vy_samp,
		double	vtheta_samp
	)

Generate and score a single trajectory.

Parameters:

x	The x position of the robot
y	The y position of the robot
theta	The orientation of the robot
vx	The x velocity of the robot
vy	The y velocity of the robot
vtheta	The theta velocity of the robot
vx_samp	The x velocity used to seed the trajectory
vy_samp	The y velocity used to seed the trajectory
vtheta_samp	The theta velocity used to seed the trajectory

Returns:: The score (as double)

Definition at line 698 of file trajectory_planner.cpp.

void iri_ackermann_local_planner::TrajectoryPlanner::updatePlan	(	const std::vector< geometry_msgs::PoseStamped > &	new_plan,
		bool	compute_dists = `false`
	)