00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
00038 #define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
00039
00040 #include <vector>
00041 #include <cmath>
00042
00043
00044 #include <costmap_2d/costmap_2d.h>
00045 #include <costmap_2d/cost_values.h>
00046 #include <base_local_planner/footprint_helper.h>
00047
00048 #include <base_local_planner/world_model.h>
00049 #include <base_local_planner/trajectory.h>
00050 #include <base_local_planner/Position2DInt.h>
00051 #include <base_local_planner/BaseLocalPlannerConfig.h>
00052
00053
00054 #include <geometry_msgs/PoseStamped.h>
00055 #include <geometry_msgs/Point.h>
00056
00057
00058 #include <tf/transform_datatypes.h>
00059
00060
00061 #include <base_local_planner/map_cell.h>
00062 #include <base_local_planner/map_grid.h>
00063
00064 namespace base_local_planner {
00069 class TrajectoryPlanner{
00070 friend class TrajectoryPlannerTest;
00071 public:
00108 TrajectoryPlanner(WorldModel& world_model,
00109 const costmap_2d::Costmap2D& costmap,
00110 std::vector<geometry_msgs::Point> footprint_spec,
00111 double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
00112 double sim_time = 1.0, double sim_granularity = 0.025,
00113 int vx_samples = 20, int vtheta_samples = 20,
00114 double pdist_scale = 0.6, double gdist_scale = 0.8, double occdist_scale = 0.2,
00115 double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05,
00116 double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2,
00117 bool holonomic_robot = true,
00118 double max_vel_x = 0.5, double min_vel_x = 0.1,
00119 double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4,
00120 double backup_vel = -0.1,
00121 bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1,
00122 bool meter_scoring = true,
00123 bool simple_attractor = false,
00124 std::vector<double> y_vels = std::vector<double>(0),
00125 double stop_time_buffer = 0.2,
00126 double sim_period = 0.1, double angular_sim_granularity = 0.025);
00127
00131 ~TrajectoryPlanner();
00132
00136 void reconfigure(BaseLocalPlannerConfig &cfg);
00137
00145 Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
00146 tf::Stamped<tf::Pose>& drive_velocities);
00147
00153 void updatePlan(const std::vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists = false);
00154
00160 void getLocalGoal(double& x, double& y);
00161
00175 bool checkTrajectory(double x, double y, double theta, double vx, double vy,
00176 double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
00177
00191 double scoreTrajectory(double x, double y, double theta, double vx, double vy,
00192 double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
00193
00204 bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
00205
00207 void setFootprint( std::vector<geometry_msgs::Point> footprint ) { footprint_spec_ = footprint; }
00208
00210 geometry_msgs::Polygon getFootprintPolygon() const { return costmap_2d::toPolygon(footprint_spec_); }
00211 std::vector<geometry_msgs::Point> getFootprint() const { return footprint_spec_; }
00212
00213 private:
00227 Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
00228 double acc_x, double acc_y, double acc_theta);
00229
00247 void generateTrajectory(double x, double y, double theta, double vx, double vy,
00248 double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
00249 double acc_theta, double impossible_cost, Trajectory& traj);
00250
00258 double footprintCost(double x_i, double y_i, double theta_i);
00259
00260 base_local_planner::FootprintHelper footprint_helper_;
00261
00262 MapGrid path_map_;
00263 MapGrid goal_map_;
00264 const costmap_2d::Costmap2D& costmap_;
00265 WorldModel& world_model_;
00266
00267 std::vector<geometry_msgs::Point> footprint_spec_;
00268
00269 std::vector<geometry_msgs::PoseStamped> global_plan_;
00270
00271 bool stuck_left, stuck_right;
00272 bool rotating_left, rotating_right;
00273
00274 bool stuck_left_strafe, stuck_right_strafe;
00275 bool strafe_right, strafe_left;
00276
00277 bool escaping_;
00278 bool meter_scoring_;
00279
00280 double goal_x_,goal_y_;
00281
00282 double final_goal_x_, final_goal_y_;
00283 bool final_goal_position_valid_;
00284
00285 double sim_time_;
00286 double sim_granularity_;
00287 double angular_sim_granularity_;
00288
00289 int vx_samples_;
00290 int vtheta_samples_;
00291
00292 double pdist_scale_, gdist_scale_, occdist_scale_;
00293 double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
00294
00295 double prev_x_, prev_y_;
00296 double escape_x_, escape_y_, escape_theta_;
00297
00298 Trajectory traj_one, traj_two;
00299
00300 double heading_lookahead_;
00301 double oscillation_reset_dist_;
00302 double escape_reset_dist_, escape_reset_theta_;
00303 bool holonomic_robot_;
00304
00305 double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_;
00306
00307 double backup_vel_;
00308
00309 bool dwa_;
00310 bool heading_scoring_;
00311 double heading_scoring_timestep_;
00312 bool simple_attractor_;
00313
00314 std::vector<double> y_vels_;
00315
00316 double stop_time_buffer_;
00317 double sim_period_;
00318
00319 double inscribed_radius_, circumscribed_radius_;
00320
00321 boost::mutex configuration_mutex_;
00322
00332 inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
00333 return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
00334 }
00335
00345 inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
00346 return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
00347 }
00348
00356 inline double computeNewThetaPosition(double thetai, double vth, double dt){
00357 return thetai + vth * dt;
00358 }
00359
00360
00369 inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
00370 if((vg - vi) >= 0) {
00371 return std::min(vg, vi + a_max * dt);
00372 }
00373 return std::max(vg, vi - a_max * dt);
00374 }
00375
00376 void getMaxSpeedToStopInTime(double time, double& vx, double& vy, double& vth){
00377 vx = acc_lim_x_ * std::max(time, 0.0);
00378 vy = acc_lim_y_ * std::max(time, 0.0);
00379 vth = acc_lim_theta_ * std::max(time, 0.0);
00380 }
00381
00382 double lineCost(int x0, int x1, int y0, int y1);
00383 double pointCost(int x, int y);
00384 double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
00385 };
00386 };
00387
00388 #endif