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

#include <safe_trajectory_planner.h>

Public Member Functions
base_local_planner::Trajectory	findBestPath (tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > user_vel, tf::Stamped< tf::Pose > &drive_velocities)
	Given the current position, orientation, and velocity of the robot, return a trajectory to follow.
base_local_planner::Trajectory	findPath (tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > user_vel)
	Given the current position, orientation, and velocity of the robot, return a trajectory to follow.
	SafeTrajectoryPlanner (base_local_planner::WorldModel &world_model, const costmap_2d::Costmap2D &costmap, std::vector< geometry_msgs::Point > footprint_spec, double inscribed_radius, double circumscribed_radius, double acc_lim_x=1.0, double acc_lim_y=1.0, double acc_lim_theta=1.0, double sim_time=1.0, double sim_granularity=0.025, int vx_samples=10, int vy_samples=10, int vtheta_samples=10, double userdist_scale=0.8, double occdist_scale=0.2, double max_vel_x=0.5, double min_vel_x=0.1, double max_vel_y=0.2, double min_vel_y=-0.2, double max_vel_th=1.0, double min_vel_th=-1.0, bool holonomic_robot=true, bool dwa=false)
	Constructs a trajectory controller.
	~SafeTrajectoryPlanner ()
	Destructs a trajectory controller.
Private Member Functions
double	computeNewThetaPosition (double thetai, double vth, double dt)
	Compute orientation based on velocity.
double	computeNewVelocity (double vg, double vi, double a_max, double dt)
	Compute velocity based on acceleration.
double	computeNewXPosition (double xi, double vx, double vy, double theta, double dt)
	Compute x position based on velocity.
double	computeNewYPosition (double yi, double vx, double vy, double theta, double dt)
	Compute y position based on velocity.
base_local_planner::Trajectory	createTrajectories (double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta, double dx, double dy, double dtheta)
	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, double dx, double dy, double dtheta, base_local_planner::Trajectory &traj)
	Generate and score a single trajectory.
void	getFillCells (std::vector< base_local_planner::Position2DInt > &footprint)
	Fill the outline of a polygon, in this case the robot footprint, in a grid.
std::vector < base_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< base_local_planner::Position2DInt > &pts)
	Use Bresenham's algorithm to trace a line between two points in a grid.
double	lineCost (int x0, int x1, int y0, int y1)
double	pointCost (int x, int y)
Private Attributes
double	acc_lim_theta_
	The acceleration limits of the robot.
double	acc_lim_x_
double	acc_lim_y_
double	circumscribed_radius_
	The inscribed and circumscribed radii of the robot.
const costmap_2d::Costmap2D &	costmap_
	Provides access to cost map information.
bool	dwa_
std::vector< geometry_msgs::Point >	footprint_spec_
	The footprint specification of the robot.
bool	holonomic_robot_
double	inscribed_radius_
base_local_planner::MapGrid	map_
	The local map grid where we propagate goal and path distance.
double	max_vel_th_
double	max_vel_x_
double	max_vel_y_
double	min_vel_th_
	Velocity limits for the controller.
double	min_vel_x_
double	min_vel_y_
double	occdist_scale_
	Scaling factors for the controller's cost function.
double	sim_granularity_
	The distance between simulation points.
double	sim_time_
	The number of seconds each trajectory is "rolled-out".
base_local_planner::Trajectory	traj_one
base_local_planner::Trajectory	traj_two
	Used for scoring trajectories.
double	userdist_scale_
	<
int	vtheta_samples_
	<
int	vx_samples_
int	vy_samples_
	<
base_local_planner::WorldModel &	world_model_
	The world model that the controller uses for collision detection.
Friends
class	SafeTrajectoryPlannerTest

Detailed Description

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

Definition at line 72 of file safe_trajectory_planner.h.

Constructor & Destructor Documentation

safe_teleop::SafeTrajectoryPlanner::SafeTrajectoryPlanner	(	base_local_planner::WorldModel &	world_model,
		const costmap_2d::Costmap2D &	costmap,
		std::vector< geometry_msgs::Point >	footprint_spec,
		double	inscribed_radius,
		double	circumscribed_radius,
		double	acc_lim_x = `1.0`,
		double	acc_lim_y = `1.0`,
		double	acc_lim_theta = `1.0`,
		double	sim_time = `1.0`,
		double	sim_granularity = `0.025`,
		int	vx_samples = `10`,
		int	vy_samples = `10`,
		int	vtheta_samples = `10`,
		double	userdist_scale = `0.8`,
		double	occdist_scale = `0.2`,
		double	max_vel_x = `0.5`,
		double	min_vel_x = `0.1`,
		double	max_vel_y = `0.2`,
		double	min_vel_y = `-0.2`,
		double	max_vel_th = `1.0`,
		double	min_vel_th = `-1.0`,
		bool	holonomic_robot = `true`,
		bool	dwa = `false`
	)

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
vy_samples	The number of trajectories to sample in the y dimension
vtheta_samples	The number of trajectories to sample in the theta dimension
userdist_scale	A scaling factor for how close the robot should stay to the user's commanded path
heading_scale	A scaling factor for how close the robot should stay to the user's commanded direction
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
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_y	The maximum y velocity the controller will explore
min_vel_y	The minimum y 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
holonomic_robot	Set to true if robot is holonomic
dwa	Set this to true to use the Dynamic Window Approach, false to use acceleration limits

Definition at line 8 of file safe_trajectory_planner.cpp.

safe_teleop::SafeTrajectoryPlanner::~SafeTrajectoryPlanner ( )

Destructs a trajectory controller.

Definition at line 34 of file safe_trajectory_planner.cpp.

Member Function Documentation

double safe_teleop::SafeTrajectoryPlanner::computeNewThetaPosition	(	double	thetai,
		double	vth,
		double	dt
	)		`[inline, private]`

Compute orientation based on velocity.

Parameters:

thetai	The current orientation
vth	The current theta velocity
dt	The timestep to take

Returns:: The new orientation

Definition at line 282 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::computeNewVelocity	(	double	vg,
		double	vi,
		double	a_max,
		double	dt
	)		`[inline, private]`

Compute velocity based on acceleration.

Parameters:

vg	The desired velocity, what we're accelerating up to
vi	The current velocity
a_max	An acceleration limit
dt	The timestep to take

Returns:: The new velocity

Definition at line 295 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::computeNewXPosition	(	double	xi,
		double	vx,
		double	vy,
		double	theta,
		double	dt
	)		`[inline, private]`

Compute x position based on velocity.

Parameters:

xi	The current x position
vx	The current x velocity
vy	The current y velocity
theta	The current orientation
dt	The timestep to take

Returns:: The new x position

Definition at line 258 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::computeNewYPosition	(	double	yi,
		double	vx,
		double	vy,
		double	theta,
		double	dt
	)		`[inline, private]`

Compute y position based on velocity.

Parameters:

yi	The current y position
vx	The current x velocity
vy	The current y velocity
theta	The current orientation
dt	The timestep to take

Returns:: The new y position

Definition at line 271 of file safe_trajectory_planner.h.

Trajectory safe_teleop::SafeTrajectoryPlanner::createTrajectories	(	double	x,
		double	y,
		double	theta,
		double	vx,
		double	vy,
		double	vtheta,
		double	acc_x,
		double	acc_y,
		double	acc_theta,
		double	dx,
		double	dy,
		double	dtheta
	)		`[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
dx	The desire x velocity
dy	The desired y velocity
dtheta	The desired theta velocity

Returns:

Definition at line 214 of file safe_trajectory_planner.cpp.

Trajectory safe_teleop::SafeTrajectoryPlanner::findBestPath	(	tf::Stamped< tf::Pose >	global_pose,
		tf::Stamped< tf::Pose >	global_vel,
		tf::Stamped< tf::Pose >	user_vel,
		tf::Stamped< tf::Pose > &	drive_velocities
	)

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 361 of file safe_trajectory_planner.cpp.

Trajectory safe_teleop::SafeTrajectoryPlanner::findPath	(	tf::Stamped< tf::Pose >	global_pose,
		tf::Stamped< tf::Pose >	global_vel,
		tf::Stamped< tf::Pose >	user_vel
	)

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
user_vel	The velocities to use to construct the path

Returns:: The selected path or trajectory

Definition at line 410 of file safe_trajectory_planner.cpp.

double safe_teleop::SafeTrajectoryPlanner::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 449 of file safe_trajectory_planner.cpp.

void safe_teleop::SafeTrajectoryPlanner::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,
		double	dx,
		double	dy,
		double	dtheta,
		base_local_planner::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
dx	The desire x velocity
dy	The desired y velocity
dtheta	The desired theta velocity
traj	Will be set to the generated trajectory with its associated score

Definition at line 37 of file safe_trajectory_planner.cpp.

void safe_teleop::SafeTrajectoryPlanner::getFillCells ( std::vector< base_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

Definition at line 600 of file safe_trajectory_planner.cpp.

vector< base_local_planner::Position2DInt > safe_teleop::SafeTrajectoryPlanner::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 543 of file safe_trajectory_planner.cpp.

void safe_teleop::SafeTrajectoryPlanner::getLineCells	(	int	x0,
		int	x1,
		int	y0,
		int	y1,
		std::vector< base_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