Public Member Functions | Private Member Functions | Private Attributes | Friends | List of all members
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>

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. More...
 
Trajectory findBestPath (const geometry_msgs::PoseStamped &global_pose, geometry_msgs::PoseStamped &global_vel, geometry_msgs::PoseStamped &drive_velocities)
 Given the current position, orientation, and velocity of the robot, return a trajectory to follow. More...
 
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. More...
 
std::vector< geometry_msgs::PointgetFootprint () const
 
geometry_msgs::Polygon getFootprintPolygon () const
 Return the footprint specification of the robot. More...
 
void getLocalGoal (double &x, double &y)
 Accessor for the goal the robot is currently pursuing in world corrdinates. More...
 
void reconfigure (BaseLocalPlannerConfig &cfg)
 Reconfigures the trajectory planner. More...
 
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. More...
 
void setFootprint (std::vector< geometry_msgs::Point > footprint)
 Set the footprint specification of the robot. More...
 
 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 path_distance_bias=0.6, double goal_distance_bias=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. More...
 
void updatePlan (const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
 Update the plan that the controller is following. More...
 
 ~TrajectoryPlanner ()
 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...
 
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. 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, Trajectory &traj)
 Generate and score a single trajectory. More...
 
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. More...
 
double acc_lim_x_
 
double acc_lim_y_
 
double angular_sim_granularity_
 The distance between angular simulation points. More...
 
double backup_vel_
 The velocity to use while backing up. More...
 
double circumscribed_radius_
 
boost::mutex configuration_mutex_
 
const costmap_2d::Costmap2Dcostmap_
 Provides access to cost map information. More...
 
bool dwa_
 Should we use the dynamic window approach? More...
 
double escape_reset_dist_
 
double escape_reset_theta_
 The distance the robot must travel before it can leave escape mode. More...
 
double escape_theta_
 Used to calculate the distance the robot has traveled before reseting escape booleans. More...
 
double escape_x_
 
double escape_y_
 
bool escaping_
 Boolean to keep track of whether we're in escape mode. More...
 
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. More...
 
double final_goal_x_
 
double final_goal_y_
 The end position of the plan. More...
 
base_local_planner::FootprintHelper footprint_helper_
 
std::vector< geometry_msgs::Pointfootprint_spec_
 The footprint specification of the robot. More...
 
std::vector< geometry_msgs::PoseStamped > global_plan_
 The global path for the robot to follow. More...
 
double goal_distance_bias_
 
MapGrid goal_map_
 The local map grid where we propagate goal distance. More...
 
double goal_x_
 
double goal_y_
 Storage for the local goal the robot is pursuing. More...
 
double heading_lookahead_
 How far the robot should look ahead of itself when differentiating between different rotational velocities. More...
 
bool heading_scoring_
 Should we score based on the rollout approach or the heading approach. More...
 
double heading_scoring_timestep_
 How far to look ahead in time when we score a heading. More...
 
bool holonomic_robot_
 Is the robot holonomic or not? More...
 
double inscribed_radius_
 
double max_vel_th_
 
double max_vel_x_
 
bool meter_scoring_
 
double min_in_place_vel_th_
 Velocity limits for the controller. More...
 
double min_vel_th_
 
double min_vel_x_
 
double occdist_scale_
 Scaling factors for the controller's cost function. More...
 
double oscillation_reset_dist_
 The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past. More...
 
double path_distance_bias_
 
MapGrid path_map_
 The local map grid where we propagate path distance. More...
 
double prev_x_
 
double prev_y_
 Used to calculate the distance the robot has traveled before reseting oscillation booleans. More...
 
bool rotating_left
 
bool rotating_right
 Booleans to keep track of the direction of rotation for the robot. More...
 
double sim_granularity_
 The distance between simulation points. More...
 
double sim_period_
 The number of seconds to use to compute max/min vels for dwa. More...
 
double sim_time_
 The number of seconds each trajectory is "rolled-out". More...
 
bool simple_attractor_
 Enables simple attraction to a goal point. More...
 
double stop_time_buffer_
 How long before hitting something we're going to enforce that the robot stop. More...
 
bool strafe_left
 Booleans to keep track of strafe direction for the robot. More...
 
bool strafe_right
 
bool stuck_left
 
bool stuck_left_strafe
 
bool stuck_right
 Booleans to keep the robot from oscillating during rotation. More...
 
bool stuck_right_strafe
 Booleans to keep the robot from oscillating during strafing. More...
 
Trajectory traj_one
 
Trajectory traj_two
 Used for scoring trajectories. More...
 
int vtheta_samples_
 The number of samples we'll take in the theta dimension of the control space. More...
 
int vx_samples_
 The number of samples we'll take in the x dimenstion of the control space. More...
 
WorldModelworld_model_
 The world model that the controller uses for collision detection. More...
 
std::vector< double > y_vels_
 Y velocities to explore. More...
 

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

Constructor & Destructor Documentation

◆ TrajectoryPlanner()

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  path_distance_bias = 0.6,
double  goal_distance_bias = 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
path_distance_biasA scaling factor for how close the robot should stay to the path
goal_distance_biasA 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 142 of file trajectory_planner.cpp.

◆ ~TrajectoryPlanner()

base_local_planner::TrajectoryPlanner::~TrajectoryPlanner ( )

Destructs a trajectory controller.

Definition at line 191 of file trajectory_planner.cpp.

Member Function Documentation

◆ checkTrajectory()

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 503 of file trajectory_planner.cpp.

◆ computeNewThetaPosition()

double base_local_planner::TrajectoryPlanner::computeNewThetaPosition ( double  thetai,
double  vth,
double  dt 
)
inlineprivate

Compute orientation based on velocity.

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

Definition at line 388 of file trajectory_planner.h.

◆ computeNewVelocity()

double base_local_planner::TrajectoryPlanner::computeNewVelocity ( double  vg,
double  vi,
double  a_max,
double  dt 
)
inlineprivate

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

◆ computeNewXPosition()

double base_local_planner::TrajectoryPlanner::computeNewXPosition ( double  xi,
double  vx,
double  vy,
double  theta,
double  dt 
)
inlineprivate

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

◆ computeNewYPosition()

double base_local_planner::TrajectoryPlanner::computeNewYPosition ( double  yi,
double  vx,
double  vy,
double  theta,
double  dt 
)
inlineprivate

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

◆ createTrajectories()

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 536 of file trajectory_planner.cpp.

◆ findBestPath()

Trajectory base_local_planner::TrajectoryPlanner::findBestPath ( const geometry_msgs::PoseStamped &  global_pose,
geometry_msgs::PoseStamped &  global_vel,
geometry_msgs::PoseStamped &  drive_velocities 
)

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 907 of file trajectory_planner.cpp.

◆ footprintCost()

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 988 of file trajectory_planner.cpp.

◆ generateTrajectory()

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 214 of file trajectory_planner.cpp.

◆ getCellCosts()

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 193 of file trajectory_planner.cpp.

◆ getFootprint()

std::vector<geometry_msgs::Point> base_local_planner::TrajectoryPlanner::getFootprint ( ) const
inline

Definition at line 243 of file trajectory_planner.h.

◆ getFootprintPolygon()

geometry_msgs::Polygon base_local_planner::TrajectoryPlanner::getFootprintPolygon ( ) const
inline

Return the footprint specification of the robot.

Definition at line 242 of file trajectory_planner.h.

◆ getLocalGoal()

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 994 of file trajectory_planner.cpp.

◆ getMaxSpeedToStopInTime()

void base_local_planner::TrajectoryPlanner::getMaxSpeedToStopInTime ( double  time,
double &  vx,
double &  vy,
double &  vth 
)
inlineprivate

Definition at line 408 of file trajectory_planner.h.

◆ headingDiff()

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

Definition at line 372 of file trajectory_planner.cpp.

◆ lineCost()

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

Definition at line 389 of file trajectory_planner.cpp.

◆ pointCost()

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

Definition at line 466 of file trajectory_planner.cpp.

◆ reconfigure()

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

Reconfigures the trajectory planner.

Definition at line 61 of file trajectory_planner.cpp.

◆ scoreTrajectory()

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 519 of file trajectory_planner.cpp.

◆ setFootprint()

void base_local_planner::TrajectoryPlanner::setFootprint ( std::vector< geometry_msgs::Point footprint)
inline

Set the footprint specification of the robot.

Definition at line 239 of file trajectory_planner.h.

◆ updatePlan()

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 476 of file trajectory_planner.cpp.

Friends And Related Function Documentation

◆ TrajectoryPlannerTest

friend class TrajectoryPlannerTest
friend

Definition at line 102 of file trajectory_planner.h.

Member Data Documentation

◆ acc_lim_theta_

double base_local_planner::TrajectoryPlanner::acc_lim_theta_
private

The acceleration limits of the robot.

Definition at line 325 of file trajectory_planner.h.

◆ acc_lim_x_

double base_local_planner::TrajectoryPlanner::acc_lim_x_
private

Definition at line 325 of file trajectory_planner.h.

◆ acc_lim_y_

double base_local_planner::TrajectoryPlanner::acc_lim_y_
private

Definition at line 325 of file trajectory_planner.h.

◆ angular_sim_granularity_

double base_local_planner::TrajectoryPlanner::angular_sim_granularity_
private

The distance between angular simulation points.

Definition at line 319 of file trajectory_planner.h.

◆ backup_vel_

double base_local_planner::TrajectoryPlanner::backup_vel_
private

The velocity to use while backing up.

Definition at line 339 of file trajectory_planner.h.

◆ circumscribed_radius_

double base_local_planner::TrajectoryPlanner::circumscribed_radius_
private

Definition at line 351 of file trajectory_planner.h.

◆ configuration_mutex_

boost::mutex base_local_planner::TrajectoryPlanner::configuration_mutex_
private

Definition at line 353 of file trajectory_planner.h.

◆ costmap_

const costmap_2d::Costmap2D& base_local_planner::TrajectoryPlanner::costmap_
private

Provides access to cost map information.

Definition at line 296 of file trajectory_planner.h.

◆ dwa_

bool base_local_planner::TrajectoryPlanner::dwa_
private

Should we use the dynamic window approach?

Definition at line 341 of file trajectory_planner.h.

◆ escape_reset_dist_

double base_local_planner::TrajectoryPlanner::escape_reset_dist_
private

Definition at line 334 of file trajectory_planner.h.

◆ escape_reset_theta_

double base_local_planner::TrajectoryPlanner::escape_reset_theta_
private

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

Definition at line 334 of file trajectory_planner.h.

◆ escape_theta_

double base_local_planner::TrajectoryPlanner::escape_theta_
private

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

Definition at line 328 of file trajectory_planner.h.

◆ escape_x_

double base_local_planner::TrajectoryPlanner::escape_x_
private

Definition at line 328 of file trajectory_planner.h.

◆ escape_y_

double base_local_planner::TrajectoryPlanner::escape_y_
private

Definition at line 328 of file trajectory_planner.h.

◆ escaping_

bool base_local_planner::TrajectoryPlanner::escaping_
private

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

Definition at line 309 of file trajectory_planner.h.

◆ final_goal_position_valid_

bool base_local_planner::TrajectoryPlanner::final_goal_position_valid_
private

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

Definition at line 315 of file trajectory_planner.h.

◆ final_goal_x_

double base_local_planner::TrajectoryPlanner::final_goal_x_
private

Definition at line 314 of file trajectory_planner.h.

◆ final_goal_y_

double base_local_planner::TrajectoryPlanner::final_goal_y_
private

The end position of the plan.

Definition at line 314 of file trajectory_planner.h.

◆ footprint_helper_

base_local_planner::FootprintHelper base_local_planner::TrajectoryPlanner::footprint_helper_
private

Definition at line 292 of file trajectory_planner.h.

◆ footprint_spec_

std::vector<geometry_msgs::Point> base_local_planner::TrajectoryPlanner::footprint_spec_
private

The footprint specification of the robot.

Definition at line 299 of file trajectory_planner.h.

◆ global_plan_

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

The global path for the robot to follow.

Definition at line 301 of file trajectory_planner.h.

◆ goal_distance_bias_

double base_local_planner::TrajectoryPlanner::goal_distance_bias_
private

Definition at line 324 of file trajectory_planner.h.

◆ goal_map_

MapGrid base_local_planner::TrajectoryPlanner::goal_map_
private

The local map grid where we propagate goal distance.

Definition at line 295 of file trajectory_planner.h.

◆ goal_x_

double base_local_planner::TrajectoryPlanner::goal_x_
private

Definition at line 312 of file trajectory_planner.h.

◆ goal_y_

double base_local_planner::TrajectoryPlanner::goal_y_
private

Storage for the local goal the robot is pursuing.

Definition at line 312 of file trajectory_planner.h.

◆ heading_lookahead_

double base_local_planner::TrajectoryPlanner::heading_lookahead_
private

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

Definition at line 332 of file trajectory_planner.h.

◆ heading_scoring_

bool base_local_planner::TrajectoryPlanner::heading_scoring_
private

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

Definition at line 342 of file trajectory_planner.h.

◆ heading_scoring_timestep_

double base_local_planner::TrajectoryPlanner::heading_scoring_timestep_
private

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

Definition at line 343 of file trajectory_planner.h.

◆ holonomic_robot_

bool base_local_planner::TrajectoryPlanner::holonomic_robot_
private

Is the robot holonomic or not?

Definition at line 335 of file trajectory_planner.h.

◆ inscribed_radius_

double base_local_planner::TrajectoryPlanner::inscribed_radius_
private

Definition at line 351 of file trajectory_planner.h.

◆ max_vel_th_

double base_local_planner::TrajectoryPlanner::max_vel_th_
private

Definition at line 337 of file trajectory_planner.h.

◆ max_vel_x_

double base_local_planner::TrajectoryPlanner::max_vel_x_
private

Definition at line 337 of file trajectory_planner.h.

◆ meter_scoring_

bool base_local_planner::TrajectoryPlanner::meter_scoring_
private

Definition at line 310 of file trajectory_planner.h.

◆ min_in_place_vel_th_

double base_local_planner::TrajectoryPlanner::min_in_place_vel_th_
private

Velocity limits for the controller.

Definition at line 337 of file trajectory_planner.h.

◆ min_vel_th_

double base_local_planner::TrajectoryPlanner::min_vel_th_
private

Definition at line 337 of file trajectory_planner.h.

◆ min_vel_x_

double base_local_planner::TrajectoryPlanner::min_vel_x_
private

Definition at line 337 of file trajectory_planner.h.

◆ occdist_scale_

double base_local_planner::TrajectoryPlanner::occdist_scale_
private

Scaling factors for the controller's cost function.

Definition at line 324 of file trajectory_planner.h.

◆ oscillation_reset_dist_

double base_local_planner::TrajectoryPlanner::oscillation_reset_dist_
private

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

Definition at line 333 of file trajectory_planner.h.

◆ path_distance_bias_

double base_local_planner::TrajectoryPlanner::path_distance_bias_
private

Definition at line 324 of file trajectory_planner.h.

◆ path_map_

MapGrid base_local_planner::TrajectoryPlanner::path_map_
private

The local map grid where we propagate path distance.

Definition at line 294 of file trajectory_planner.h.

◆ prev_x_

double base_local_planner::TrajectoryPlanner::prev_x_
private

Definition at line 327 of file trajectory_planner.h.

◆ prev_y_

double base_local_planner::TrajectoryPlanner::prev_y_
private

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

Definition at line 327 of file trajectory_planner.h.

◆ rotating_left

bool base_local_planner::TrajectoryPlanner::rotating_left
private

Definition at line 304 of file trajectory_planner.h.

◆ rotating_right

bool base_local_planner::TrajectoryPlanner::rotating_right
private

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

Definition at line 304 of file trajectory_planner.h.

◆ sim_granularity_

double base_local_planner::TrajectoryPlanner::sim_granularity_
private

The distance between simulation points.

Definition at line 318 of file trajectory_planner.h.

◆ sim_period_

double base_local_planner::TrajectoryPlanner::sim_period_
private

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

Definition at line 349 of file trajectory_planner.h.

◆ sim_time_

double base_local_planner::TrajectoryPlanner::sim_time_
private

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

Definition at line 317 of file trajectory_planner.h.

◆ simple_attractor_

bool base_local_planner::TrajectoryPlanner::simple_attractor_
private

Enables simple attraction to a goal point.

Definition at line 344 of file trajectory_planner.h.

◆ stop_time_buffer_

double base_local_planner::TrajectoryPlanner::stop_time_buffer_
private

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

Definition at line 348 of file trajectory_planner.h.

◆ strafe_left

bool base_local_planner::TrajectoryPlanner::strafe_left
private

Booleans to keep track of strafe direction for the robot.

Definition at line 307 of file trajectory_planner.h.

◆ strafe_right

bool base_local_planner::TrajectoryPlanner::strafe_right
private

Definition at line 307 of file trajectory_planner.h.

◆ stuck_left

bool base_local_planner::TrajectoryPlanner::stuck_left
private

Definition at line 303 of file trajectory_planner.h.

◆ stuck_left_strafe

bool base_local_planner::TrajectoryPlanner::stuck_left_strafe
private

Definition at line 306 of file trajectory_planner.h.

◆ stuck_right

bool base_local_planner::TrajectoryPlanner::stuck_right
private

Booleans to keep the robot from oscillating during rotation.

Definition at line 303 of file trajectory_planner.h.

◆ stuck_right_strafe

bool base_local_planner::TrajectoryPlanner::stuck_right_strafe
private

Booleans to keep the robot from oscillating during strafing.

Definition at line 306 of file trajectory_planner.h.

◆ traj_one

Trajectory base_local_planner::TrajectoryPlanner::traj_one
private

Definition at line 330 of file trajectory_planner.h.

◆ traj_two

Trajectory base_local_planner::TrajectoryPlanner::traj_two
private

Used for scoring trajectories.

Definition at line 330 of file trajectory_planner.h.

◆ vtheta_samples_

int base_local_planner::TrajectoryPlanner::vtheta_samples_
private

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

Definition at line 322 of file trajectory_planner.h.

◆ vx_samples_

int base_local_planner::TrajectoryPlanner::vx_samples_
private

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

Definition at line 321 of file trajectory_planner.h.

◆ world_model_

WorldModel& base_local_planner::TrajectoryPlanner::world_model_
private

The world model that the controller uses for collision detection.

Definition at line 297 of file trajectory_planner.h.

◆ y_vels_

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

Y velocities to explore.

Definition at line 346 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, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:24