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) : costmap_(costmap) {
00046 if (costmap != NULL) {
00047 world_model_ = new base_local_planner::CostmapModel(*costmap_);
00048 }
00049 }
00050
00051 ObstacleCostFunction::~ObstacleCostFunction() {
00052 if (world_model_ != NULL) {
00053 delete world_model_;
00054 }
00055 }
00056
00057
00058 void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
00059
00060 max_trans_vel_ = max_trans_vel;
00061 max_scaling_factor_ = max_scaling_factor;
00062 scaling_speed_ = scaling_speed;
00063 }
00064
00065 void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
00066 footprint_spec_ = footprint_spec;
00067 }
00068
00069 bool ObstacleCostFunction::prepare() {
00070 return true;
00071 }
00072
00073 double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
00074 double cost = 0;
00075 double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
00076 double px, py, pth;
00077 if (footprint_spec_.size() == 0) {
00078
00079 ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
00080 return -9;
00081 }
00082
00083 for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
00084 traj.getPoint(i, px, py, pth);
00085 cost = footprintCost(px, py, pth,
00086 scale,
00087 footprint_spec_,
00088 costmap_,
00089 world_model_);
00090 }
00091 return cost;
00092 }
00093
00094 double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
00095 double vmag = sqrt(traj.xv_ * traj.xv_ + traj.yv_ * traj.yv_);
00096
00097
00098
00099 double scale = 1.0;
00100 if (vmag > scaling_speed) {
00101
00102 double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
00103 scale = max_scaling_factor * ratio + 1.0;
00104 }
00105 return scale;
00106 }
00107
00108 double ObstacleCostFunction::footprintCost (
00109 const double& x,
00110 const double& y,
00111 const double& th,
00112 double scale,
00113 std::vector<geometry_msgs::Point>& footprint_spec,
00114 costmap_2d::Costmap2D* costmap,
00115 base_local_planner::WorldModel* world_model) {
00116 double cos_th = cos(th);
00117 double sin_th = sin(th);
00118
00119 double occ_cost = 0.0;
00120
00121 std::vector<geometry_msgs::Point> scaled_oriented_footprint;
00122 for(unsigned int i = 0; i < footprint_spec.size(); ++i) {
00123 geometry_msgs::Point new_pt;
00124 new_pt.x = x + (scale * footprint_spec[i].x * cos_th - scale * footprint_spec[i].y * sin_th);
00125 new_pt.y = y + (scale * footprint_spec[i].x * sin_th + scale * footprint_spec[i].y * cos_th);
00126 scaled_oriented_footprint.push_back(new_pt);
00127 geometry_msgs::Point robot_position;
00128 robot_position.x = x;
00129 robot_position.y = y;
00130
00131
00132 double footprint_cost = world_model->footprintCost(robot_position,
00133 scaled_oriented_footprint,
00134 costmap->getInscribedRadius(),
00135 costmap->getCircumscribedRadius());
00136
00137 if (footprint_cost < 0) {
00138 return -6.0;
00139 }
00140 unsigned int cell_x, cell_y;
00141
00142
00143 if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
00144 return -7.0;
00145 }
00146
00147 occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
00148
00149 }
00150
00151 return occ_cost;
00152 }
00153
00154 }