Public Member Functions | Private Member Functions | Private Attributes | Friends
safe_teleop::SafeTrajectoryPlanner Class Reference

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>

List of all members.

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::Costmap2Dcostmap_
 Provides access to cost map information.
bool dwa_
std::vector< geometry_msgs::Pointfootprint_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::WorldModelworld_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_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
vy_samplesThe number of trajectories to sample in the y dimension
vtheta_samplesThe number of trajectories to sample in the theta dimension
userdist_scaleA scaling factor for how close the robot should stay to the user's commanded path
heading_scaleA scaling factor for how close the robot should stay to the user's commanded direction
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
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_yThe maximum y velocity the controller will explore
min_vel_yThe minimum y 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
holonomic_robotSet to true if robot is holonomic
dwaSet this to true to use the Dynamic Window Approach, false to use acceleration limits

Definition at line 8 of file safe_trajectory_planner.cpp.

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:
thetaiThe current orientation
vthThe current theta velocity
dtThe 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:
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 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:
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 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:
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 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:
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
dxThe desire x velocity
dyThe desired y velocity
dthetaThe 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_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 361 of file safe_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
user_velThe 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_iThe x position of the robot
y_iThe y position of the robot
theta_iThe 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:
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
dxThe desire x velocity
dyThe desired y velocity
dthetaThe desired theta velocity
trajWill 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:
footprintThe 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_iThe x position of the robot
y_iThe y position of the robot
theta_iThe orientation of the robot
fillIf 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:
x0The x coordinate of the first point
x1The x coordinate of the second point
y0The y coordinate of the first point
y1The y coordinate of the second point
ptsWill 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

The acceleration limits of the robot.

Definition at line 238 of file safe_trajectory_planner.h.

Definition at line 238 of file safe_trajectory_planner.h.

Definition at line 238 of file safe_trajectory_planner.h.

The inscribed and circumscribed radii of the robot.

Definition at line 228 of file safe_trajectory_planner.h.

Provides access to cost map information.

Definition at line 224 of file safe_trajectory_planner.h.

Definition at line 246 of file safe_trajectory_planner.h.

The footprint specification of the robot.

Definition at line 227 of file safe_trajectory_planner.h.

Definition at line 244 of file safe_trajectory_planner.h.

Definition at line 228 of file safe_trajectory_planner.h.

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

Definition at line 223 of file safe_trajectory_planner.h.

Definition at line 242 of file safe_trajectory_planner.h.

Definition at line 242 of file safe_trajectory_planner.h.

Definition at line 242 of file safe_trajectory_planner.h.

Velocity limits for the controller.

Definition at line 242 of file safe_trajectory_planner.h.

Definition at line 242 of file safe_trajectory_planner.h.

Definition at line 242 of file safe_trajectory_planner.h.

Scaling factors for the controller's cost function.

Definition at line 237 of file safe_trajectory_planner.h.

The distance between simulation points.

Definition at line 231 of file safe_trajectory_planner.h.

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

Definition at line 230 of file safe_trajectory_planner.h.

Definition at line 240 of file safe_trajectory_planner.h.

Used for scoring trajectories.

Definition at line 240 of file safe_trajectory_planner.h.

<

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.

<

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.

Definition at line 233 of file safe_trajectory_planner.h.

<

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.

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:


safe_teleop_base
Author(s):
autogenerated on Thu Mar 14 2019 02:49:46