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/map_grid_cost_function.h>
00039
00040 namespace base_local_planner {
00041
00042 MapGridCostFunction::MapGridCostFunction(costmap_2d::Costmap2D* costmap,
00043 double xshift,
00044 double yshift,
00045 bool is_local_goal_function,
00046 CostAggregationType aggregationType) :
00047 costmap_(costmap),
00048 map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()),
00049 aggregationType_(aggregationType),
00050 xshift_(xshift),
00051 yshift_(yshift),
00052 is_local_goal_function_(is_local_goal_function),
00053 stop_on_failure_(true) {}
00054
00055 void MapGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
00056 target_poses_ = target_poses;
00057 }
00058
00059 bool MapGridCostFunction::prepare() {
00060 map_.resetPathDist();
00061
00062 if (is_local_goal_function_) {
00063 map_.setLocalGoal(*costmap_, target_poses_);
00064 } else {
00065 map_.setTargetCells(*costmap_, target_poses_);
00066 }
00067 return true;
00068 }
00069
00070 double MapGridCostFunction::getCellCosts(unsigned int px, unsigned int py) {
00071 double grid_dist = map_(px, py).target_dist;
00072 return grid_dist;
00073 }
00074
00075 double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
00076 double cost = 0.0;
00077 if (aggregationType_ == Product) {
00078 cost = 1.0;
00079 }
00080 double px, py, pth;
00081 unsigned int cell_x, cell_y;
00082 double grid_dist;
00083
00084 for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
00085 traj.getPoint(i, px, py, pth);
00086
00087
00088 if (xshift_ != 0.0) {
00089 px = px + xshift_ * cos(pth);
00090 py = py + xshift_ * sin(pth);
00091 }
00092
00093 if (yshift_ != 0.0) {
00094 px = px + yshift_ * cos(pth + M_PI_2);
00095 py = py + yshift_ * sin(pth + M_PI_2);
00096 }
00097
00098
00099 if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
00100
00101 ROS_WARN("Off Map %f, %f", px, py);
00102 return -4.0;
00103 }
00104 grid_dist = getCellCosts(cell_x, cell_y);
00105
00106 if (stop_on_failure_) {
00107 if (grid_dist == map_.obstacleCosts()) {
00108 return -3.0;
00109 } else if (grid_dist == map_.unreachableCellCosts()) {
00110 return -2.0;
00111 }
00112 }
00113
00114 switch( aggregationType_ ) {
00115 case Last:
00116 cost = grid_dist;
00117 break;
00118 case Sum:
00119 cost += grid_dist;
00120 break;
00121 case Product:
00122 if (cost > 0) {
00123 cost *= grid_dist;
00124 }
00125 break;
00126 }
00127 }
00128 return cost;
00129 }
00130
00131 }