Public Member Functions | Private Member Functions | Private Attributes | Friends
base_local_planner::TrajectoryPlanner Class Reference

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

#include <trajectory_planner.h>

List of all members.

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)
 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 (BaseLocalPlannerConfig &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 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=20, int vtheta_samples=20, double pdist_scale=0.6, double gdist_scale=0.8, double occdist_scale=0.2, double heading_lookahead=0.325, double oscillation_reset_dist=0.05, double escape_reset_dist=0.10, double escape_reset_theta=M_PI_2, bool holonomic_robot=true, double max_vel_x=0.5, double min_vel_x=0.1, double max_vel_th=1.0, double min_vel_th=-1.0, double min_in_place_vel_th=0.4, double backup_vel=-0.1, bool dwa=false, bool heading_scoring=false, double heading_scoring_timestep=0.1, bool meter_scoring=true, bool simple_attractor=false, std::vector< double > y_vels=std::vector< double >(0), double stop_time_buffer=0.2, double sim_period=0.1, double angular_sim_granularity=0.025)
 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

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.
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 getMaxSpeedToStopInTime (double time, double &vx, double &vy, double &vth)
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 acc_lim_theta_
 The acceleration limits of the robot.
double acc_lim_x_
double acc_lim_y_
double angular_sim_granularity_
 The distance between angular simulation points.
double backup_vel_
 The velocity to use while backing up.
boost::mutex configuration_mutex_
const costmap_2d::Costmap2Dcostmap_
 Provides access to cost map information.
bool dwa_
 Should we use the dynamic window approach?
double escape_reset_dist_
double escape_reset_theta_
 The distance the robot must travel before it can leave escape mode.
double escape_theta_
 Used to calculate the distance the robot has traveled before reseting escape booleans.
double escape_x_
double escape_y_
bool escaping_
 Boolean to keep track of whether we're in escape mode.
bool final_goal_position_valid_
 True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent.
double final_goal_x_
double final_goal_y_
 The end position of the plan.
base_local_planner::FootprintHelper footprint_helper_
std::vector< geometry_msgs::Pointfootprint_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.
MapGrid goal_map_
 The local map grid where we propagate goal distance.
double goal_x_
double goal_y_
 Storage for the local goal the robot is pursuing.
double heading_lookahead_
 How far the robot should look ahead of itself when differentiating between different rotational velocities.
bool heading_scoring_
 Should we score based on the rollout approach or the heading approach.
double heading_scoring_timestep_
 How far to look ahead in time when we score a heading.
bool holonomic_robot_
 Is the robot holonomic or not?
double max_vel_th_
double max_vel_x_
bool meter_scoring_
double min_in_place_vel_th_
 Velocity limits for the controller.
double min_vel_th_
double min_vel_x_
double occdist_scale_
 Scaling factors for the controller's cost function.
double oscillation_reset_dist_
 The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past.
MapGrid path_map_
 The local map grid where we propagate path distance.
double pdist_scale_
double prev_x_
double prev_y_
 Used to calculate the distance the robot has traveled before reseting oscillation booleans.
bool rotating_left
bool rotating_right
 Booleans to keep track of the direction of rotation for the robot.
double sim_granularity_
 The distance between simulation points.
double sim_period_
 The number of seconds to use to compute max/min vels for dwa.
double sim_time_
 The number of seconds each trajectory is "rolled-out".
bool simple_attractor_
 Enables simple attraction to a goal point.
double stop_time_buffer_
 How long before hitting something we're going to enforce that the robot stop.
bool strafe_left
 Booleans to keep track of strafe direction for the robot.
bool strafe_right
bool stuck_left
bool stuck_left_strafe
bool stuck_right
 Booleans to keep the robot from oscillating during rotation.
bool stuck_right_strafe
 Booleans to keep the robot from oscillating during strafing.
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.
WorldModelworld_model_
 The world model that the controller uses for collision detection.
std::vector< double > y_vels_
 Y velocities to explore.

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 69 of file trajectory_planner.h.


Constructor & Destructor Documentation

base_local_planner::TrajectoryPlanner::TrajectoryPlanner ( WorldModel world_model,
const costmap_2d::Costmap2D costmap,
std::vector< geometry_msgs::Point footprint_spec,
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 = 20,
int  vtheta_samples = 20,
double  pdist_scale = 0.6,
double  gdist_scale = 0.8,
double  occdist_scale = 0.2,
double  heading_lookahead = 0.325,
double  oscillation_reset_dist = 0.05,
double  escape_reset_dist = 0.10,
double  escape_reset_theta = M_PI_2,
bool  holonomic_robot = true,
double  max_vel_x = 0.5,
double  min_vel_x = 0.1,
double  max_vel_th = 1.0,
double  min_vel_th = -1.0,
double  min_in_place_vel_th = 0.4,
double  backup_vel = -0.1,
bool  dwa = false,
bool  heading_scoring = false,
double  heading_scoring_timestep = 0.1,
bool  meter_scoring = true,
bool  simple_attractor = false,
std::vector< double >  y_vels = std::vector<double>(0),
double  stop_time_buffer = 0.2,
double  sim_period = 0.1,
double  angular_sim_granularity = 0.025 
)

Constructs a trajectory controller.

Parameters:
world_modelThe WorldModel the trajectory controller uses to check for collisions
costmapA reference to the Costmap the controller should use
footprint_specA polygon representing the footprint of the robot. (Must be convex)
inscribed_radiusThe radius of the inscribed circle of the robot
circumscribed_radiusThe radius of the circumscribed circle of the robot
acc_lim_xThe acceleration limit of the robot in the x direction
acc_lim_yThe acceleration limit of the robot in the y direction
acc_lim_thetaThe acceleration limit of the robot in the theta direction
sim_timeThe number of seconds to "roll-out" each trajectory
sim_granularityThe distance between simulation points should be small enough that the robot doesn't hit things
vx_samplesThe number of trajectories to sample in the x dimension
vtheta_samplesThe number of trajectories to sample in the theta dimension
pdist_scaleA scaling factor for how close the robot should stay to the path
gdist_scaleA scaling factor for how aggresively the robot should pursue a local goal
occdist_scaleA scaling factor for how much the robot should prefer to stay away from obstacles
heading_lookaheadHow far the robot should look ahead of itself when differentiating between different rotational velocities
oscillation_reset_distThe distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
escape_reset_distThe distance the robot must travel before it can exit escape mode
escape_reset_thetaThe distance the robot must rotate before it can exit escape mode
holonomic_robotSet this to true if the robot being controlled can take y velocities and false otherwise
max_vel_xThe maximum x velocity the controller will explore
min_vel_xThe minimum x velocity the controller will explore
max_vel_thThe maximum rotational velocity the controller will explore
min_vel_thThe minimum rotational velocity the controller will explore
min_in_place_vel_thThe absolute value of the minimum in-place rotational velocity the controller will explore
backup_velThe velocity to use while backing up
dwaSet this to true to use the Dynamic Window Approach, false to use acceleration limits
heading_scoringSet this to true to score trajectories based on the robot's heading after 1 timestep
heading_scoring_timestepHow far to look ahead in time when we score heading based trajectories
meter_scoringadapt parameters to costmap resolution
simple_attractorSet this to true to allow simple attraction to a goal point instead of intelligent cost propagation
y_velsA vector of the y velocities the controller will explore
angular_sim_granularityThe distance between simulation points for angular velocity should be small enough that the robot doesn't hit things

Definition at line 141 of file trajectory_planner.cpp.

Destructs a trajectory controller.

Definition at line 187 of file trajectory_planner.cpp.


Member Function Documentation

bool base_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:
xThe x position of the robot
yThe y position of the robot
thetaThe orientation of the robot
vxThe x velocity of the robot
vyThe y velocity of the robot
vthetaThe theta velocity of the robot
vx_sampThe x velocity used to seed the trajectory
vy_sampThe y velocity used to seed the trajectory
vtheta_sampThe theta velocity used to seed the trajectory
Returns:
True if the trajectory is legal, false otherwise

Definition at line 512 of file trajectory_planner.cpp.

double base_local_planner::TrajectoryPlanner::computeNewThetaPosition ( double  thetai,
double  vth,
double  dt 
) [inline, private]

Compute orientation based on velocity.

Parameters:
thetaiThe current orientation
vthThe current theta velocity
dtThe timestep to take
Returns:
The new orientation

Definition at line 346 of file trajectory_planner.h.

double base_local_planner::TrajectoryPlanner::computeNewVelocity ( double  vg,
double  vi,
double  a_max,
double  dt 
) [inline, private]

Compute velocity based on acceleration.

Parameters:
vgThe desired velocity, what we're accelerating up to
viThe current velocity
a_maxAn acceleration limit
dtThe timestep to take
Returns:
The new velocity

Definition at line 359 of file trajectory_planner.h.

double base_local_planner::TrajectoryPlanner::computeNewXPosition ( double  xi,
double  vx,
double  vy,
double  theta,
double  dt 
) [inline, private]

Compute x position based on velocity.

Parameters:
xiThe current x position
vxThe current x velocity
vyThe current y velocity
thetaThe current orientation
dtThe timestep to take
Returns:
The new x position

Definition at line 322 of file trajectory_planner.h.

double base_local_planner::TrajectoryPlanner::computeNewYPosition ( double  yi,
double  vx,
double  vy,
double  theta,
double  dt 
) [inline, private]

Compute y position based on velocity.

Parameters:
yiThe current y position
vxThe current x velocity
vyThe current y velocity
thetaThe current orientation
dtThe timestep to take
Returns:
The new y position

Definition at line 335 of file trajectory_planner.h.

Trajectory base_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:
xThe x position of the robot
yThe y position of the robot
thetaThe orientation of the robot
vxThe x velocity of the robot
vyThe y velocity of the robot
vthetaThe theta velocity of the robot
acc_xThe x acceleration limit of the robot
acc_yThe y acceleration limit of the robot
acc_thetaThe theta acceleration limit of the robot
Returns:

Definition at line 545 of file trajectory_planner.cpp.

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

Parameters:
global_poseThe current pose of the robot in world space
global_velThe current velocity of the robot in world space
drive_velocitiesWill be set to velocities to send to the robot base
Returns:
The selected path or trajectory

Definition at line 918 of file trajectory_planner.cpp.

double base_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_iThe x position of the robot
y_iThe y position of the robot
theta_iThe orientation of the robot
Returns:

Definition at line 992 of file trajectory_planner.cpp.

void base_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:
xThe x position of the robot
yThe y position of the robot
thetaThe orientation of the robot
vxThe x velocity of the robot
vyThe y velocity of the robot
vthetaThe theta velocity of the robot
vx_sampThe x velocity used to seed the trajectory
vy_sampThe y velocity used to seed the trajectory
vtheta_sampThe theta velocity used to seed the trajectory
acc_xThe x acceleration limit of the robot
acc_yThe y acceleration limit of the robot
acc_thetaThe theta acceleration limit of the robot
impossible_costThe cost value of a cell in the local map grid that is considered impassable
trajWill be set to the generated trajectory with its associated score

create and score a trajectory given the current pose of the robot and selected velocities

Definition at line 210 of file trajectory_planner.cpp.

bool base_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:
cxThe x coordinate of the cell in the map grid
cyThe y coordinate of the cell in the map grid
path_costWill be set to the path distance component of the cost function
goal_costWill be set to the goal distance component of the cost function
occ_costWill be set to the costmap value of the cell
total_costWill 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 189 of file trajectory_planner.cpp.

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

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

Parameters:
xWill be set to the x position of the local goal
yWill be set to the y position of the local goal

Definition at line 1015 of file trajectory_planner.cpp.

void base_local_planner::TrajectoryPlanner::getMaxSpeedToStopInTime ( double  time,
double &  vx,
double &  vy,
double &  vth 
) [inline, private]

Definition at line 366 of file trajectory_planner.h.

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

Definition at line 368 of file trajectory_planner.cpp.

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

Definition at line 398 of file trajectory_planner.cpp.

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

Definition at line 475 of file trajectory_planner.cpp.

void base_local_planner::TrajectoryPlanner::reconfigure ( BaseLocalPlannerConfig &  cfg)

Reconfigures the trajectory planner.

Definition at line 59 of file trajectory_planner.cpp.

double base_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:
xThe x position of the robot
yThe y position of the robot
thetaThe orientation of the robot
vxThe x velocity of the robot
vyThe y velocity of the robot
vthetaThe theta velocity of the robot
vx_sampThe x velocity used to seed the trajectory
vy_sampThe y velocity used to seed the trajectory
vtheta_sampThe theta velocity used to seed the trajectory
Returns:
The score (as double)

Definition at line 528 of file trajectory_planner.cpp.

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

Update the plan that the controller is following.

Parameters:
new_planA new plan for the controller to follow
compute_distsWheter or not to compute path/goal distances when a plan is updated

Definition at line 485 of file trajectory_planner.cpp.


Friends And Related Function Documentation

friend class TrajectoryPlannerTest [friend]

Definition at line 70 of file trajectory_planner.h.


Member Data Documentation

The acceleration limits of the robot.

Definition at line 285 of file trajectory_planner.h.

Definition at line 285 of file trajectory_planner.h.

Definition at line 285 of file trajectory_planner.h.

The distance between angular simulation points.

Definition at line 279 of file trajectory_planner.h.

The velocity to use while backing up.

Definition at line 299 of file trajectory_planner.h.

Definition at line 311 of file trajectory_planner.h.

Provides access to cost map information.

Definition at line 256 of file trajectory_planner.h.

Should we use the dynamic window approach?

Definition at line 301 of file trajectory_planner.h.

Definition at line 294 of file trajectory_planner.h.

The distance the robot must travel before it can leave escape mode.

Definition at line 294 of file trajectory_planner.h.

Used to calculate the distance the robot has traveled before reseting escape booleans.

Definition at line 288 of file trajectory_planner.h.

Definition at line 288 of file trajectory_planner.h.

Definition at line 288 of file trajectory_planner.h.

Boolean to keep track of whether we're in escape mode.

Definition at line 269 of file trajectory_planner.h.

True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent.

Definition at line 275 of file trajectory_planner.h.

Definition at line 274 of file trajectory_planner.h.

The end position of the plan.

Definition at line 274 of file trajectory_planner.h.

Definition at line 252 of file trajectory_planner.h.

The footprint specification of the robot.

Definition at line 259 of file trajectory_planner.h.

Definition at line 284 of file trajectory_planner.h.

std::vector<geometry_msgs::PoseStamped> base_local_planner::TrajectoryPlanner::global_plan_ [private]

The global path for the robot to follow.

Definition at line 261 of file trajectory_planner.h.

The local map grid where we propagate goal distance.

Definition at line 255 of file trajectory_planner.h.

Definition at line 272 of file trajectory_planner.h.

Storage for the local goal the robot is pursuing.

Definition at line 272 of file trajectory_planner.h.

How far the robot should look ahead of itself when differentiating between different rotational velocities.

Definition at line 292 of file trajectory_planner.h.

Should we score based on the rollout approach or the heading approach.

Definition at line 302 of file trajectory_planner.h.

How far to look ahead in time when we score a heading.

Definition at line 303 of file trajectory_planner.h.

Is the robot holonomic or not?

Definition at line 295 of file trajectory_planner.h.

Definition at line 297 of file trajectory_planner.h.

Definition at line 297 of file trajectory_planner.h.

Definition at line 270 of file trajectory_planner.h.

Velocity limits for the controller.

Definition at line 297 of file trajectory_planner.h.

Definition at line 297 of file trajectory_planner.h.

Definition at line 297 of file trajectory_planner.h.

Scaling factors for the controller's cost function.

Definition at line 284 of file trajectory_planner.h.

The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past.

Definition at line 293 of file trajectory_planner.h.

The local map grid where we propagate path distance.

Definition at line 254 of file trajectory_planner.h.

Definition at line 284 of file trajectory_planner.h.

Definition at line 287 of file trajectory_planner.h.

Used to calculate the distance the robot has traveled before reseting oscillation booleans.

Definition at line 287 of file trajectory_planner.h.

Definition at line 264 of file trajectory_planner.h.

Booleans to keep track of the direction of rotation for the robot.

Definition at line 264 of file trajectory_planner.h.

The distance between simulation points.

Definition at line 278 of file trajectory_planner.h.

The number of seconds to use to compute max/min vels for dwa.

Definition at line 309 of file trajectory_planner.h.

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

Definition at line 277 of file trajectory_planner.h.

Enables simple attraction to a goal point.

Definition at line 304 of file trajectory_planner.h.

How long before hitting something we're going to enforce that the robot stop.

Definition at line 308 of file trajectory_planner.h.

Booleans to keep track of strafe direction for the robot.

Definition at line 267 of file trajectory_planner.h.

Definition at line 267 of file trajectory_planner.h.

Definition at line 263 of file trajectory_planner.h.

Definition at line 266 of file trajectory_planner.h.

Booleans to keep the robot from oscillating during rotation.

Definition at line 263 of file trajectory_planner.h.

Booleans to keep the robot from oscillating during strafing.

Definition at line 266 of file trajectory_planner.h.

Definition at line 290 of file trajectory_planner.h.

Used for scoring trajectories.

Definition at line 290 of file trajectory_planner.h.

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

Definition at line 282 of file trajectory_planner.h.

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

Definition at line 281 of file trajectory_planner.h.

The world model that the controller uses for collision detection.

Definition at line 257 of file trajectory_planner.h.

std::vector<double> base_local_planner::TrajectoryPlanner::y_vels_ [private]

Y velocities to explore.

Definition at line 306 of file trajectory_planner.h.


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


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko
autogenerated on Mon Oct 6 2014 02:45:34