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
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 private:
00219 Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
00220 double acc_x, double acc_y, double acc_theta);
00221
00239 void generateTrajectory(double x, double y, double theta, double vx, double vy,
00240 double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
00241 double acc_theta, double impossible_cost, Trajectory& traj);
00242
00250 double footprintCost(double x_i, double y_i, double theta_i);
00251
00252 base_local_planner::FootprintHelper footprint_helper_;
00253
00254 MapGrid path_map_;
00255 MapGrid goal_map_;
00256 const costmap_2d::Costmap2D& costmap_;
00257 WorldModel& world_model_;
00258
00259 std::vector<geometry_msgs::Point> footprint_spec_;
00260
00261 std::vector<geometry_msgs::PoseStamped> global_plan_;
00262
00263 bool stuck_left, stuck_right;
00264 bool rotating_left, rotating_right;
00265
00266 bool stuck_left_strafe, stuck_right_strafe;
00267 bool strafe_right, strafe_left;
00268
00269 bool escaping_;
00270 bool meter_scoring_;
00271
00272 double goal_x_,goal_y_;
00273
00274 double final_goal_x_, final_goal_y_;
00275 bool final_goal_position_valid_;
00276
00277 double sim_time_;
00278 double sim_granularity_;
00279 double angular_sim_granularity_;
00280
00281 int vx_samples_;
00282 int vtheta_samples_;
00283
00284 double pdist_scale_, gdist_scale_, occdist_scale_;
00285 double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
00286
00287 double prev_x_, prev_y_;
00288 double escape_x_, escape_y_, escape_theta_;
00289
00290 Trajectory traj_one, traj_two;
00291
00292 double heading_lookahead_;
00293 double oscillation_reset_dist_;
00294 double escape_reset_dist_, escape_reset_theta_;
00295 bool holonomic_robot_;
00296
00297 double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_;
00298
00299 double backup_vel_;
00300
00301 bool dwa_;
00302 bool heading_scoring_;
00303 double heading_scoring_timestep_;
00304 bool simple_attractor_;
00305
00306 std::vector<double> y_vels_;
00307
00308 double stop_time_buffer_;
00309 double sim_period_;
00310
00311 boost::mutex configuration_mutex_;
00312
00322 inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
00323 return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
00324 }
00325
00335 inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
00336 return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
00337 }
00338
00346 inline double computeNewThetaPosition(double thetai, double vth, double dt){
00347 return thetai + vth * dt;
00348 }
00349
00350
00359 inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
00360 if((vg - vi) >= 0) {
00361 return std::min(vg, vi + a_max * dt);
00362 }
00363 return std::max(vg, vi - a_max * dt);
00364 }
00365
00366 void getMaxSpeedToStopInTime(double time, double& vx, double& vy, double& vth){
00367 vx = acc_lim_x_ * std::max(time, 0.0);
00368 vy = acc_lim_y_ * std::max(time, 0.0);
00369 vth = acc_lim_theta_ * std::max(time, 0.0);
00370 }
00371
00372 double lineCost(int x0, int x1, int y0, int y1);
00373 double pointCost(int x, int y);
00374 double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
00375 };
00376 };
00377
00378 #endif