map_grid.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 #include <base_local_planner/map_grid.h>
00035 #include <costmap_2d/cost_values.h>
00036 using namespace std;
00037 
00038 namespace base_local_planner{
00039 
00040   MapGrid::MapGrid()
00041     : size_x_(0), size_y_(0)
00042   {
00043   }
00044 
00045   MapGrid::MapGrid(unsigned int size_x, unsigned int size_y) 
00046     : size_x_(size_x), size_y_(size_y)
00047   {
00048     commonInit();
00049   }
00050 
00051   MapGrid::MapGrid(const MapGrid& mg){
00052     size_y_ = mg.size_y_;
00053     size_x_ = mg.size_x_;
00054     map_ = mg.map_;
00055   }
00056 
00057   void MapGrid::commonInit(){
00058     //don't allow construction of zero size grid
00059     ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
00060 
00061     map_.resize(size_y_ * size_x_);
00062 
00063     //make each cell aware of its location in the grid
00064     for(unsigned int i = 0; i < size_y_; ++i){
00065       for(unsigned int j = 0; j < size_x_; ++j){
00066         unsigned int id = size_x_ * i + j;
00067         map_[id].cx = j;
00068         map_[id].cy = i;
00069       }
00070     }
00071   }
00072 
00073   size_t MapGrid::getIndex(int x, int y){
00074     return size_x_ * y + x;
00075   }
00076 
00077   MapGrid& MapGrid::operator= (const MapGrid& mg){
00078     size_y_ = mg.size_y_;
00079     size_x_ = mg.size_x_;
00080     map_ = mg.map_;
00081     return *this;
00082   }
00083 
00084   void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y){
00085     if(map_.size() != size_x * size_y)
00086       map_.resize(size_x * size_y);
00087 
00088     if(size_x_ != size_x || size_y_ != size_y){
00089       size_x_ = size_x;
00090       size_y_ = size_y;
00091 
00092       for(unsigned int i = 0; i < size_y_; ++i){
00093         for(unsigned int j = 0; j < size_x_; ++j){
00094           int index = size_x_ * i + j;
00095           map_[index].cx = j;
00096           map_[index].cy = i;
00097         }
00098       }
00099     }
00100   }
00101 
00102 
00103   inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
00104       const costmap_2d::Costmap2D& costmap){
00105 
00106     //if the cell is an obstacle set the max path distance
00107     unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
00108     if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
00109         (cost == costmap_2d::LETHAL_OBSTACLE ||
00110          cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
00111          cost == costmap_2d::NO_INFORMATION)){
00112       check_cell->target_dist = obstacleCosts();
00113       return false;
00114     }
00115 
00116     double new_target_dist = current_cell->target_dist + 1;
00117     if (new_target_dist < check_cell->target_dist) {
00118       check_cell->target_dist = new_target_dist;
00119     }
00120     return true;
00121   }
00122 
00123 
00124   //reset the path_dist and goal_dist fields for all cells
00125   void MapGrid::resetPathDist(){
00126     for(unsigned int i = 0; i < map_.size(); ++i) {
00127       map_[i].target_dist = unreachableCellCosts();
00128       map_[i].target_mark = false;
00129       map_[i].within_robot = false;
00130     }
00131   }
00132 
00133   void MapGrid::adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
00134       std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution) {
00135     if (global_plan_in.size() == 0) {
00136       return;
00137     }
00138     double last_x = global_plan_in[0].pose.position.x;
00139     double last_y = global_plan_in[0].pose.position.y;
00140     global_plan_out.push_back(global_plan_in[0]);
00141 
00142     // we can take "holes" in the plan smaller than 2 grid cells (squared = 4)
00143     double min_sq_resolution = resolution * resolution * 4;
00144 
00145     for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
00146       double loop_x = global_plan_in[i].pose.position.x;
00147       double loop_y = global_plan_in[i].pose.position.y;
00148       double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
00149       if (sqdist > min_sq_resolution) {
00150         int steps = ((sqrt(sqdist) - sqrt(min_sq_resolution)) / resolution) - 1;
00151         // add a points in-between
00152         double deltax = (loop_x - last_x) / steps;
00153         double deltay = (loop_y - last_y) / steps;
00154         // TODO: Interpolate orientation
00155         for (int j = 1; j < steps; ++j) {
00156           geometry_msgs::PoseStamped pose;
00157           pose.pose.position.x = last_x + j * deltax;
00158           pose.pose.position.y = last_y + j * deltay;
00159           pose.pose.position.z = global_plan_in[i].pose.position.z;
00160           pose.pose.orientation = global_plan_in[i].pose.orientation;
00161           pose.header = global_plan_in[i].header;
00162           global_plan_out.push_back(pose);
00163         }
00164       }
00165       global_plan_out.push_back(global_plan_in[i]);
00166       last_x = loop_x;
00167       last_y = loop_y;
00168     }
00169   }
00170 
00171   //update what map cells are considered path based on the global_plan
00172   void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
00173       const std::vector<geometry_msgs::PoseStamped>& global_plan) {
00174     sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
00175 
00176     bool started_path = false;
00177 
00178     queue<MapCell*> path_dist_queue;
00179 
00180     std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
00181     adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
00182     if (adjusted_global_plan.size() != global_plan.size()) {
00183       ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
00184     }
00185     unsigned int i;
00186     // put global path points into local map until we reach the border of the local map
00187     for (i = 0; i < adjusted_global_plan.size(); ++i) {
00188       double g_x = adjusted_global_plan[i].pose.position.x;
00189       double g_y = adjusted_global_plan[i].pose.position.y;
00190       unsigned int map_x, map_y;
00191       if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
00192         MapCell& current = getCell(map_x, map_y);
00193         current.target_dist = 0.0;
00194         current.target_mark = true;
00195         path_dist_queue.push(&current);
00196         started_path = true;
00197       } else if (started_path) {
00198           break;
00199       }
00200     }
00201     if (!started_path) {
00202       ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
00203           i, adjusted_global_plan.size(), global_plan.size());
00204       return;
00205     }
00206 
00207     computeTargetDistance(path_dist_queue, costmap);
00208   }
00209 
00210   //mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
00211   void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,
00212       const std::vector<geometry_msgs::PoseStamped>& global_plan) {
00213     sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
00214 
00215     int local_goal_x = -1;
00216     int local_goal_y = -1;
00217     bool started_path = false;
00218 
00219     std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
00220     adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
00221 
00222     // skip global path points until we reach the border of the local map
00223     for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
00224       double g_x = adjusted_global_plan[i].pose.position.x;
00225       double g_y = adjusted_global_plan[i].pose.position.y;
00226       unsigned int map_x, map_y;
00227       if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
00228         local_goal_x = map_x;
00229         local_goal_y = map_y;
00230         started_path = true;
00231       } else {
00232         if (started_path) {
00233           break;
00234         }// else we might have a non pruned path, so we just continue
00235       }
00236     }
00237     if (!started_path) {
00238       ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
00239       return;
00240     }
00241 
00242     queue<MapCell*> path_dist_queue;
00243     if (local_goal_x >= 0 && local_goal_y >= 0) {
00244       MapCell& current = getCell(local_goal_x, local_goal_y);
00245       costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
00246       current.target_dist = 0.0;
00247       current.target_mark = true;
00248       path_dist_queue.push(&current);
00249     }
00250 
00251     computeTargetDistance(path_dist_queue, costmap);
00252   }
00253 
00254 
00255 
00256   void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
00257     MapCell* current_cell;
00258     MapCell* check_cell;
00259     unsigned int last_col = size_x_ - 1;
00260     unsigned int last_row = size_y_ - 1;
00261     while(!dist_queue.empty()){
00262       current_cell = dist_queue.front();
00263 
00264 
00265       dist_queue.pop();
00266 
00267       if(current_cell->cx > 0){
00268         check_cell = current_cell - 1;
00269         if(!check_cell->target_mark){
00270           //mark the cell as visisted
00271           check_cell->target_mark = true;
00272           if(updatePathCell(current_cell, check_cell, costmap)) {
00273             dist_queue.push(check_cell);
00274           }
00275         }
00276       }
00277 
00278       if(current_cell->cx < last_col){
00279         check_cell = current_cell + 1;
00280         if(!check_cell->target_mark){
00281           check_cell->target_mark = true;
00282           if(updatePathCell(current_cell, check_cell, costmap)) {
00283             dist_queue.push(check_cell);
00284           }
00285         }
00286       }
00287 
00288       if(current_cell->cy > 0){
00289         check_cell = current_cell - size_x_;
00290         if(!check_cell->target_mark){
00291           check_cell->target_mark = true;
00292           if(updatePathCell(current_cell, check_cell, costmap)) {
00293             dist_queue.push(check_cell);
00294           }
00295         }
00296       }
00297 
00298       if(current_cell->cy < last_row){
00299         check_cell = current_cell + size_x_;
00300         if(!check_cell->target_mark){
00301           check_cell->target_mark = true;
00302           if(updatePathCell(current_cell, check_cell, costmap)) {
00303             dist_queue.push(check_cell);
00304           }
00305         }
00306       }
00307     }
00308   }
00309 
00310 };


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08