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 (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. 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::Point > | getFootprint () 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 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. 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::Costmap2D & | costmap_ |
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::Point > | footprint_spec_ |
The footprint specification of the robot. More... | |
double | gdist_scale_ |
std::vector< geometry_msgs::PoseStamped > | global_plan_ |
The global path for the robot to follow. More... | |
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... | |
MapGrid | path_map_ |
The local map grid where we propagate path distance. More... | |
double | pdist_scale_ |
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... | |
WorldModel & | world_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 |
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.
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.
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 |
vtheta_samples | The number of trajectories to sample in the theta dimension |
pdist_scale | A scaling factor for how close the robot should stay to the path |
gdist_scale | A scaling factor for how aggresively the robot should pursue a local goal |
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 |
escape_reset_dist | The distance the robot must travel before it can exit escape mode |
escape_reset_theta | The distance the robot must rotate before it can exit escape mode |
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_th | The maximum rotational velocity the controller will explore |
min_vel_th | The minimum rotational velocity the controller will explore |
min_in_place_vel_th | The absolute value of the minimum in-place rotational velocity the controller will explore |
backup_vel | The velocity to use while backing up |
dwa | Set this to true to use the Dynamic Window Approach, false to use acceleration limits |
heading_scoring | Set this to true to score trajectories based on the robot's heading after 1 timestep |
heading_scoring_timestep | How far to look ahead in time when we score heading based trajectories |
meter_scoring | adapt parameters to costmap resolution |
simple_attractor | Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation |
y_vels | A vector of the y velocities the controller will explore |
angular_sim_granularity | The 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.
base_local_planner::TrajectoryPlanner::~TrajectoryPlanner | ( | ) |
Destructs a trajectory controller.
Definition at line 190 of file trajectory_planner.cpp.
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.
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 |
Definition at line 502 of file trajectory_planner.cpp.
|
inlineprivate |
Compute orientation based on velocity.
thetai | The current orientation |
vth | The current theta velocity |
dt | The timestep to take |
Definition at line 356 of file trajectory_planner.h.
|
inlineprivate |
Compute velocity based on acceleration.
vg | The desired velocity, what we're accelerating up to |
vi | The current velocity |
a_max | An acceleration limit |
dt | The timestep to take |
Definition at line 369 of file trajectory_planner.h.
|
inlineprivate |
Compute x position based on velocity.
xi | The current x position |
vx | The current x velocity |
vy | The current y velocity |
theta | The current orientation |
dt | The timestep to take |
Definition at line 332 of file trajectory_planner.h.
|
inlineprivate |
Compute y position based on velocity.
yi | The current y position |
vx | The current x velocity |
vy | The current y velocity |
theta | The current orientation |
dt | The timestep to take |
Definition at line 345 of file trajectory_planner.h.
|
private |
Create the trajectories we wish to explore, score them, and return the best option.
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 |
Definition at line 535 of file trajectory_planner.cpp.
Trajectory base_local_planner::TrajectoryPlanner::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.
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 |
Definition at line 906 of file trajectory_planner.cpp.
|
private |
Checks the legality of the robot footprint at a position and orientation using the world model.
x_i | The x position of the robot |
y_i | The y position of the robot |
theta_i | The orientation of the robot |
Definition at line 980 of file trajectory_planner.cpp.
|
private |
Generate and score a single trajectory.
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 |
traj | Will 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 213 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.
cx | The x coordinate of the cell in the map grid |
cy | The y coordinate of the cell in the map grid |
path_cost | Will be set to the path distance component of the cost function |
goal_cost | Will be set to the goal distance component of the cost function |
occ_cost | Will be set to the costmap value of the cell |
total_cost | Will be set to the value of the overall cost function, taking into account the scaling parameters |
Definition at line 192 of file trajectory_planner.cpp.
|
inline |
Definition at line 211 of file trajectory_planner.h.
|
inline |
Return the footprint specification of the robot.
Definition at line 210 of file trajectory_planner.h.
void base_local_planner::TrajectoryPlanner::getLocalGoal | ( | double & | x, |
double & | y | ||
) |
Accessor for the goal the robot is currently pursuing in world corrdinates.
x | Will be set to the x position of the local goal |
y | Will be set to the y position of the local goal |
Definition at line 986 of file trajectory_planner.cpp.
|
inlineprivate |
Definition at line 376 of file trajectory_planner.h.
|
private |
Definition at line 371 of file trajectory_planner.cpp.
|
private |
Definition at line 388 of file trajectory_planner.cpp.
|
private |
Definition at line 465 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.
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 |
Definition at line 518 of file trajectory_planner.cpp.
|
inline |
Set the footprint specification of the robot.
Definition at line 207 of file trajectory_planner.h.
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.
new_plan | A new plan for the controller to follow |
compute_dists | Wheter or not to compute path/goal distances when a plan is updated |
Definition at line 475 of file trajectory_planner.cpp.
|
friend |
Definition at line 70 of file trajectory_planner.h.
|
private |
The acceleration limits of the robot.
Definition at line 293 of file trajectory_planner.h.
|
private |
Definition at line 293 of file trajectory_planner.h.
|
private |
Definition at line 293 of file trajectory_planner.h.
|
private |
The distance between angular simulation points.
Definition at line 287 of file trajectory_planner.h.
|
private |
The velocity to use while backing up.
Definition at line 307 of file trajectory_planner.h.
|
private |
Definition at line 319 of file trajectory_planner.h.
|
private |
Definition at line 321 of file trajectory_planner.h.
|
private |
Provides access to cost map information.
Definition at line 264 of file trajectory_planner.h.
|
private |
Should we use the dynamic window approach?
Definition at line 309 of file trajectory_planner.h.
|
private |
Definition at line 302 of file trajectory_planner.h.
|
private |
The distance the robot must travel before it can leave escape mode.
Definition at line 302 of file trajectory_planner.h.
|
private |
Used to calculate the distance the robot has traveled before reseting escape booleans.
Definition at line 296 of file trajectory_planner.h.
|
private |
Definition at line 296 of file trajectory_planner.h.
|
private |
Definition at line 296 of file trajectory_planner.h.
|
private |
Boolean to keep track of whether we're in escape mode.
Definition at line 277 of file trajectory_planner.h.
|
private |
True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent.
Definition at line 283 of file trajectory_planner.h.
|
private |
Definition at line 282 of file trajectory_planner.h.
|
private |
The end position of the plan.
Definition at line 282 of file trajectory_planner.h.
|
private |
Definition at line 260 of file trajectory_planner.h.
|
private |
The footprint specification of the robot.
Definition at line 267 of file trajectory_planner.h.
|
private |
Definition at line 292 of file trajectory_planner.h.
|
private |
The global path for the robot to follow.
Definition at line 269 of file trajectory_planner.h.
|
private |
The local map grid where we propagate goal distance.
Definition at line 263 of file trajectory_planner.h.
|
private |
Definition at line 280 of file trajectory_planner.h.
|
private |
Storage for the local goal the robot is pursuing.
Definition at line 280 of file trajectory_planner.h.
|
private |
How far the robot should look ahead of itself when differentiating between different rotational velocities.
Definition at line 300 of file trajectory_planner.h.
|
private |
Should we score based on the rollout approach or the heading approach.
Definition at line 310 of file trajectory_planner.h.
|
private |
How far to look ahead in time when we score a heading.
Definition at line 311 of file trajectory_planner.h.
|
private |
Is the robot holonomic or not?
Definition at line 303 of file trajectory_planner.h.
|
private |
Definition at line 319 of file trajectory_planner.h.
|
private |
Definition at line 305 of file trajectory_planner.h.
|
private |
Definition at line 305 of file trajectory_planner.h.
|
private |
Definition at line 278 of file trajectory_planner.h.
|
private |
Velocity limits for the controller.
Definition at line 305 of file trajectory_planner.h.
|
private |
Definition at line 305 of file trajectory_planner.h.
|
private |
Definition at line 305 of file trajectory_planner.h.
|
private |
Scaling factors for the controller's cost function.
Definition at line 292 of file trajectory_planner.h.
|
private |
The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past.
Definition at line 301 of file trajectory_planner.h.
|
private |
The local map grid where we propagate path distance.
Definition at line 262 of file trajectory_planner.h.
|
private |
Definition at line 292 of file trajectory_planner.h.
|
private |
Definition at line 295 of file trajectory_planner.h.
|
private |
Used to calculate the distance the robot has traveled before reseting oscillation booleans.
Definition at line 295 of file trajectory_planner.h.
|
private |
Definition at line 272 of file trajectory_planner.h.
|
private |
Booleans to keep track of the direction of rotation for the robot.
Definition at line 272 of file trajectory_planner.h.
|
private |
The distance between simulation points.
Definition at line 286 of file trajectory_planner.h.
|
private |
The number of seconds to use to compute max/min vels for dwa.
Definition at line 317 of file trajectory_planner.h.
|
private |
The number of seconds each trajectory is "rolled-out".
Definition at line 285 of file trajectory_planner.h.
|
private |
Enables simple attraction to a goal point.
Definition at line 312 of file trajectory_planner.h.
|
private |
How long before hitting something we're going to enforce that the robot stop.
Definition at line 316 of file trajectory_planner.h.
|
private |
Booleans to keep track of strafe direction for the robot.
Definition at line 275 of file trajectory_planner.h.
|
private |
Definition at line 275 of file trajectory_planner.h.
|
private |
Definition at line 271 of file trajectory_planner.h.
|
private |
Definition at line 274 of file trajectory_planner.h.
|
private |
Booleans to keep the robot from oscillating during rotation.
Definition at line 271 of file trajectory_planner.h.
|
private |
Booleans to keep the robot from oscillating during strafing.
Definition at line 274 of file trajectory_planner.h.
|
private |
Definition at line 298 of file trajectory_planner.h.
|
private |
Used for scoring trajectories.
Definition at line 298 of file trajectory_planner.h.
|
private |
The number of samples we'll take in the theta dimension of the control space.
Definition at line 290 of file trajectory_planner.h.
|
private |
The number of samples we'll take in the x dimenstion of the control space.
Definition at line 289 of file trajectory_planner.h.
|
private |
The world model that the controller uses for collision detection.
Definition at line 265 of file trajectory_planner.h.
|
private |
Y velocities to explore.
Definition at line 314 of file trajectory_planner.h.