Go to the documentation of this file.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
00038 #include <base_local_planner/obstacle_cost_function.h>
00039 #include <cmath>
00040 #include <Eigen/Core>
00041 #include <ros/console.h>
00042
00043 namespace base_local_planner {
00044
00045 ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap)
00046 : costmap_(costmap), sum_scores_(false) {
00047 if (costmap != NULL) {
00048 world_model_ = new base_local_planner::CostmapModel(*costmap_);
00049 }
00050 }
00051
00052 ObstacleCostFunction::~ObstacleCostFunction() {
00053 if (world_model_ != NULL) {
00054 delete world_model_;
00055 }
00056 }
00057
00058
00059 void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
00060
00061 max_trans_vel_ = max_trans_vel;
00062 max_scaling_factor_ = max_scaling_factor;
00063 scaling_speed_ = scaling_speed;
00064 }
00065
00066 void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
00067 footprint_spec_ = footprint_spec;
00068 }
00069
00070 bool ObstacleCostFunction::prepare() {
00071 return true;
00072 }
00073
00074 double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
00075 double cost = 0;
00076 double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
00077 double px, py, pth;
00078 if (footprint_spec_.size() == 0) {
00079
00080 ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
00081 return -9;
00082 }
00083
00084 for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
00085 traj.getPoint(i, px, py, pth);
00086 double f_cost = footprintCost(px, py, pth,
00087 scale, footprint_spec_,
00088 costmap_, world_model_);
00089
00090 if(f_cost < 0){
00091 return f_cost;
00092 }
00093
00094 if(sum_scores_)
00095 cost += f_cost;
00096 else
00097 cost = f_cost;
00098 }
00099 return cost;
00100 }
00101
00102 double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
00103 double vmag = hypot(traj.xv_, traj.yv_);
00104
00105
00106
00107 double scale = 1.0;
00108 if (vmag > scaling_speed) {
00109
00110 double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
00111 scale = max_scaling_factor * ratio + 1.0;
00112 }
00113 return scale;
00114 }
00115
00116 double ObstacleCostFunction::footprintCost (
00117 const double& x,
00118 const double& y,
00119 const double& th,
00120 double scale,
00121 std::vector<geometry_msgs::Point> footprint_spec,
00122 costmap_2d::Costmap2D* costmap,
00123 base_local_planner::WorldModel* world_model) {
00124
00125
00126
00127 double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
00128
00129 if (footprint_cost < 0) {
00130 return -6.0;
00131 }
00132 unsigned int cell_x, cell_y;
00133
00134
00135 if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
00136 return -7.0;
00137 }
00138
00139 double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
00140
00141 return occ_cost;
00142 }
00143
00144 }