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. More...

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. More...

	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. More...

	~SafeTrajectoryPlanner ()
	Destructs a trajectory controller. More...

Private Member Functions
double	computeNewThetaPosition (double thetai, double vth, double dt)
	Compute orientation based on velocity. More...

double	computeNewVelocity (double vg, double vi, double a_max, double dt)
	Compute velocity based on acceleration. More...

double	computeNewXPosition (double xi, double vx, double vy, double theta, double dt)
	Compute x position based on velocity. More...

double	computeNewYPosition (double yi, double vx, double vy, double theta, double dt)
	Compute y position based on velocity. More...

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. More...

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. More...

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. More...

void	getFillCells (std::vector< base_local_planner::Position2DInt > &footprint)
	Fill the outline of a polygon, in this case the robot footprint, in a grid. More...

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. More...

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. More...

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. More...

double	acc_lim_x_

double	acc_lim_y_

double	circumscribed_radius_
	The inscribed and circumscribed radii of the robot. More...

const costmap_2d::Costmap2D &	costmap_
	Provides access to cost map information. More...

bool	dwa_

std::vector< geometry_msgs::Point >	footprint_spec_
	The footprint specification of the robot. More...

bool	holonomic_robot_

double	inscribed_radius_

base_local_planner::MapGrid	map_
	The local map grid where we propagate goal and path distance. More...

double	max_vel_th_

double	max_vel_x_

double	max_vel_y_

double	min_vel_th_
	Velocity limits for the controller. More...

double	min_vel_x_

double	min_vel_y_

double	occdist_scale_
	Scaling factors for the controller's cost function. More...

double	sim_granularity_
	The distance between simulation points. More...

double	sim_time_
	The number of seconds each trajectory is "rolled-out". More...

base_local_planner::Trajectory	traj_one

base_local_planner::Trajectory	traj_two
	Used for scoring trajectories. More...

double	userdist_scale_
	< More...

int	vtheta_samples_
	< More...

int	vx_samples_

int	vy_samples_
	< More...

base_local_planner::WorldModel &	world_model_
	The world model that the controller uses for collision detection. More...

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
	)

inlineprivate

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
	)

inlineprivate

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
	)

inlineprivate

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
	)

inlineprivate

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

Definition at line 471 of file safe_trajectory_planner.cpp.

double safe_teleop::SafeTrajectoryPlanner::lineCost	(	int	x0,
		int	x1,
		int	y0,
		int	y1
	)

private

Definition at line 124 of file safe_trajectory_planner.cpp.

double safe_teleop::SafeTrajectoryPlanner::pointCost	(	int	x,
		int	y
	)

private

Definition at line 203 of file safe_trajectory_planner.cpp.

Friends And Related Function Documentation

friend class SafeTrajectoryPlannerTest

friend

Definition at line 73 of file safe_trajectory_planner.h.

Member Data Documentation

double safe_teleop::SafeTrajectoryPlanner::acc_lim_theta_

private

The acceleration limits of the robot.

Definition at line 238 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::acc_lim_x_

private

Definition at line 238 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::acc_lim_y_

private

Definition at line 238 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::circumscribed_radius_

private

The inscribed and circumscribed radii of the robot.

Definition at line 228 of file safe_trajectory_planner.h.

const costmap_2d::Costmap2D& safe_teleop::SafeTrajectoryPlanner::costmap_

private

Provides access to cost map information.

Definition at line 224 of file safe_trajectory_planner.h.

bool safe_teleop::SafeTrajectoryPlanner::dwa_

private

Definition at line 246 of file safe_trajectory_planner.h.

std::vector<geometry_msgs::Point> safe_teleop::SafeTrajectoryPlanner::footprint_spec_

private

The footprint specification of the robot.

Definition at line 227 of file safe_trajectory_planner.h.

bool safe_teleop::SafeTrajectoryPlanner::holonomic_robot_

private

Definition at line 244 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::inscribed_radius_

private

Definition at line 228 of file safe_trajectory_planner.h.

base_local_planner::MapGrid safe_teleop::SafeTrajectoryPlanner::map_

private

The local map grid where we propagate goal and path distance.

Definition at line 223 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::max_vel_th_

private

Definition at line 242 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::max_vel_x_

private

Definition at line 242 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::max_vel_y_

private

Definition at line 242 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::min_vel_th_

private

Velocity limits for the controller.

Definition at line 242 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::min_vel_x_

private

Definition at line 242 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::min_vel_y_

private

Definition at line 242 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::occdist_scale_

private

Scaling factors for the controller's cost function.

Definition at line 237 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::sim_granularity_

private

The distance between simulation points.

Definition at line 231 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::sim_time_

private

The number of seconds each trajectory is "rolled-out".

Definition at line 230 of file safe_trajectory_planner.h.

base_local_planner::Trajectory safe_teleop::SafeTrajectoryPlanner::traj_one

private

Definition at line 240 of file safe_trajectory_planner.h.

base_local_planner::Trajectory safe_teleop::SafeTrajectoryPlanner::traj_two

private

Used for scoring trajectories.

Definition at line 240 of file safe_trajectory_planner.h.

double safe_teleop::SafeTrajectoryPlanner::userdist_scale_

private

<

The number of samples we'll take in the theta dimension of the control space

Definition at line 237 of file safe_trajectory_planner.h.

int safe_teleop::SafeTrajectoryPlanner::vtheta_samples_

private

<

The number of samples we'll take in the y dimension of the control space

Definition at line 235 of file safe_trajectory_planner.h.

int safe_teleop::SafeTrajectoryPlanner::vx_samples_

private

Definition at line 233 of file safe_trajectory_planner.h.

int safe_teleop::SafeTrajectoryPlanner::vy_samples_

private

<

The number of samples we'll take in the x dimension of the control space

Definition at line 234 of file safe_trajectory_planner.h.

base_local_planner::WorldModel& safe_teleop::SafeTrajectoryPlanner::world_model_

private

The world model that the controller uses for collision detection.

Definition at line 225 of file safe_trajectory_planner.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation