37 #ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_ 38 #define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_ 50 #include <base_local_planner/Position2DInt.h> 51 #include <base_local_planner/BaseLocalPlannerConfig.h> 54 #include <geometry_msgs/PoseStamped.h> 55 #include <geometry_msgs/Point.h> 110 std::vector<geometry_msgs::Point> footprint_spec,
111 double acc_lim_x = 1.0,
double acc_lim_y = 1.0,
double acc_lim_theta = 1.0,
112 double sim_time = 1.0,
double sim_granularity = 0.025,
113 int vx_samples = 20,
int vtheta_samples = 20,
114 double pdist_scale = 0.6,
double gdist_scale = 0.8,
double occdist_scale = 0.2,
115 double heading_lookahead = 0.325,
double oscillation_reset_dist = 0.05,
116 double escape_reset_dist = 0.10,
double escape_reset_theta = M_PI_2,
117 bool holonomic_robot =
true,
118 double max_vel_x = 0.5,
double min_vel_x = 0.1,
119 double max_vel_th = 1.0,
double min_vel_th = -1.0,
double min_in_place_vel_th = 0.4,
120 double backup_vel = -0.1,
121 bool dwa =
false,
bool heading_scoring =
false,
double heading_scoring_timestep = 0.1,
122 bool meter_scoring =
true,
123 bool simple_attractor =
false,
124 std::vector<double> y_vels = std::vector<double>(0),
125 double stop_time_buffer = 0.2,
126 double sim_period = 0.1,
double angular_sim_granularity = 0.025);
153 void updatePlan(
const std::vector<geometry_msgs::PoseStamped>& new_plan,
bool compute_dists =
false);
175 bool checkTrajectory(
double x,
double y,
double theta,
double vx,
double vy,
176 double vtheta,
double vx_samp,
double vy_samp,
double vtheta_samp);
191 double scoreTrajectory(
double x,
double y,
double theta,
double vx,
double vy,
192 double vtheta,
double vx_samp,
double vy_samp,
double vtheta_samp);
204 bool getCellCosts(
int cx,
int cy,
float &path_cost,
float &goal_cost,
float &occ_cost,
float &total_cost);
228 double acc_x,
double acc_y,
double acc_theta);
248 double vtheta,
double vx_samp,
double vy_samp,
double vtheta_samp,
double acc_x,
double acc_y,
249 double acc_theta,
double impossible_cost,
Trajectory& traj);
258 double footprintCost(
double x_i,
double y_i,
double theta_i);
333 return xi + (vx *
cos(theta) + vy *
cos(M_PI_2 + theta)) * dt;
346 return yi + (vx *
sin(theta) + vy *
sin(M_PI_2 + theta)) * dt;
357 return thetai + vth * dt;
371 return std::min(vg, vi + a_max * dt);
373 return std::max(vg, vi - a_max * dt);
377 vx = acc_lim_x_ * std::max(time, 0.0);
378 vy = acc_lim_y_ * std::max(time, 0.0);
379 vth = acc_lim_theta_ * std::max(time, 0.0);
382 double lineCost(
int x0,
int x1,
int y0,
int y1);
384 double headingDiff(
int cell_x,
int cell_y,
double x,
double y,
double heading);
int vx_samples_
The number of samples we'll take in the x dimenstion of the control space.
double stop_time_buffer_
How long before hitting something we're going to enforce that the robot stop.
Computes control velocities for a robot given a costmap, a plan, and the robot's position in the worl...
double angular_sim_granularity_
The distance between angular simulation points.
double computeNewYPosition(double yi, double vx, double vy, double theta, double dt)
Compute y position based on velocity.
double heading_scoring_timestep_
How far to look ahead in time when we score a heading.
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.
int vtheta_samples_
The number of samples we'll take in the theta dimension of the control space.
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.
base_local_planner::FootprintHelper footprint_helper_
double computeNewXPosition(double xi, double vx, double vy, double theta, double dt)
Compute x position based on velocity.
double oscillation_reset_dist_
The distance the robot must travel before it can explore rotational velocities that were unsuccessful...
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...
bool stuck_right
Booleans to keep the robot from oscillating during rotation.
const costmap_2d::Costmap2D & costmap_
Provides access to cost map information.
double heading_lookahead_
How far the robot should look ahead of itself when differentiating between different rotational veloc...
void setFootprint(std::vector< geometry_msgs::Point > footprint)
Set the footprint specification of the robot.
double goal_y_
Storage for the local goal the robot is pursuing.
~TrajectoryPlanner()
Destructs a trajectory controller.
bool holonomic_robot_
Is the robot holonomic or not?
double min_in_place_vel_th_
Velocity limits for the controller.
double computeNewThetaPosition(double thetai, double vth, double dt)
Compute orientation based on velocity.
double lineCost(int x0, int x1, int y0, int y1)
MapGrid path_map_
The local map grid where we propagate path distance.
double prev_y_
Used to calculate the distance the robot has traveled before reseting oscillation booleans...
bool dwa_
Should we use the dynamic window approach?
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.
double sim_time_
The number of seconds each trajectory is "rolled-out".
double backup_vel_
The velocity to use while backing up.
TFSIMD_FORCE_INLINE const tfScalar & y() const
double sim_period_
The number of seconds to use to compute max/min vels for dwa.
bool simple_attractor_
Enables simple attraction to a goal point.
std::vector< double > y_vels_
Y velocities to explore.
double escape_reset_theta_
The distance the robot must travel before it can leave escape mode.
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.
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
double escape_theta_
Used to calculate the distance the robot has traveled before reseting escape booleans.
WorldModel & world_model_
The world model that the controller uses for collision detection.
std::vector< geometry_msgs::PoseStamped > global_plan_
The global path for the robot to follow.
boost::mutex configuration_mutex_
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...
double occdist_scale_
Scaling factors for the controller's cost function.
MapGrid goal_map_
The local map grid where we propagate goal distance.
bool escaping_
Boolean to keep track of whether we're in escape mode.
double headingDiff(int cell_x, int cell_y, double x, double y, double heading)
std::vector< geometry_msgs::Point > getFootprint() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
double sim_granularity_
The distance between simulation points.
double acc_lim_theta_
The acceleration limits of the robot.
double circumscribed_radius_
double pointCost(int x, int y)
double escape_reset_dist_
Trajectory traj_two
Used for scoring trajectories.
geometry_msgs::Polygon toPolygon(std::vector< geometry_msgs::Point > pts)
void updatePlan(const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
Update the plan that the controller is following.
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
double final_goal_y_
The end position of the plan.
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.
bool heading_scoring_
Should we score based on the rollout approach or the heading approach.
bool stuck_right_strafe
Booleans to keep the robot from oscillating during strafing.
std::vector< geometry_msgs::Point > footprint_spec_
The footprint specification of the robot.
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...
double computeNewVelocity(double vg, double vi, double a_max, double dt)
Compute velocity based on acceleration.
void getLocalGoal(double &x, double &y)
Accessor for the goal the robot is currently pursuing in world corrdinates.
bool strafe_left
Booleans to keep track of strafe direction for the robot.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void reconfigure(BaseLocalPlannerConfig &cfg)
Reconfigures the trajectory planner.
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 getMaxSpeedToStopInTime(double time, double &vx, double &vy, double &vth)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
bool rotating_right
Booleans to keep track of the direction of rotation for the robot.
Holds a trajectory generated by considering an x, y, and theta velocity.
geometry_msgs::Polygon getFootprintPolygon() const
Return the footprint specification of the robot.