base_local_planner: base_local_planner: map

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 
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(unsigned int size_x, unsigned int size_y, double s, double x, double y)
00052     : size_x_(size_x), size_y_(size_y), scale(s), origin_x(x), origin_y(y)
00053   {
00054     commonInit();
00055   }
00056 
00057   MapGrid::MapGrid(const MapGrid& mg){
00058     size_y_ = mg.size_y_;
00059     size_x_ = mg.size_x_;
00060     map_ = mg.map_;
00061   }
00062 
00063   void MapGrid::commonInit(){
00064     //don't allow construction of zero size grid
00065     ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
00066 
00067     map_.resize(size_y_ * size_x_);
00068 
00069     //make each cell aware of its location in the grid
00070     for(unsigned int i = 0; i < size_y_; ++i){
00071       for(unsigned int j = 0; j < size_x_; ++j){
00072         unsigned int id = size_x_ * i + j;
00073         map_[id].cx = j;
00074         map_[id].cy = i;
00075       }
00076     }
00077   }
00078 
00079   size_t MapGrid::getIndex(int x, int y){
00080     return size_x_ * y + x;
00081   }
00082 
00083   MapGrid& MapGrid::operator= (const MapGrid& mg){
00084     size_y_ = mg.size_y_;
00085     size_x_ = mg.size_x_;
00086     map_ = mg.map_;
00087     return *this;
00088   }
00089 
00090   void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y, double o_x, double o_y){
00091     if(map_.size() != size_x * size_y)
00092       map_.resize(size_x * size_y);
00093 
00094     if(size_x_ != size_x || size_y_ != size_y){
00095       size_x_ = size_x;
00096       size_y_ = size_y;
00097 
00098       for(unsigned int i = 0; i < size_y_; ++i){
00099         for(unsigned int j = 0; j < size_x_; ++j){
00100           int index = size_x_ * i + j;
00101           map_[index].cx = j;
00102           map_[index].cy = i;
00103         }
00104       }
00105     }
00106     origin_x = o_x;
00107     origin_y = o_y;
00108   }
00109 
00110   //reset the path_dist and goal_dist fields for all cells
00111   void MapGrid::resetPathDist(){
00112     for(unsigned int i = 0; i < map_.size(); ++i){
00113       map_[i].path_dist = DBL_MAX;
00114       map_[i].goal_dist = DBL_MAX;
00115       map_[i].path_mark = false;
00116       map_[i].goal_mark = false;
00117       map_[i].within_robot = false;
00118     }
00119   }
00120 
00121   //update what map cells are considered path based on the global_plan
00122   void MapGrid::setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan){
00123     sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getOriginX(), costmap.getOriginY());
00124     int local_goal_x = -1;
00125     int local_goal_y = -1;
00126     bool started_path = false;
00127     queue<MapCell*> path_dist_queue;
00128     queue<MapCell*> goal_dist_queue;
00129     for(unsigned int i = 0; i < global_plan.size(); ++i){
00130       double g_x = global_plan[i].pose.position.x;
00131       double g_y = global_plan[i].pose.position.y;
00132       unsigned int map_x, map_y;
00133       if(costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION){
00134         MapCell& current = getCell(map_x, map_y);
00135         current.path_dist = 0.0;
00136         current.path_mark = true;
00137         path_dist_queue.push(&current);
00138         local_goal_x = map_x;
00139         local_goal_y = map_y;
00140         started_path = true;
00141       }
00142       else{
00143         if(started_path)
00144           break;
00145       }
00146     }
00147 
00148     if(local_goal_x >= 0 && local_goal_y >= 0){
00149       MapCell& current = getCell(local_goal_x, local_goal_y);
00150       costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
00151       current.goal_dist = 0.0;
00152       current.goal_mark = true;
00153       goal_dist_queue.push(&current);
00154     }
00155     //compute our distances
00156     computePathDistance(path_dist_queue, costmap);
00157     computeGoalDistance(goal_dist_queue, costmap);
00158   }
00159 
00160   void MapGrid::computePathDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
00161     MapCell* current_cell;
00162     MapCell* check_cell;
00163     unsigned int last_col = size_x_ - 1;
00164     unsigned int last_row = size_y_ - 1;
00165     while(!dist_queue.empty()){
00166       current_cell = dist_queue.front();
00167       check_cell = current_cell;
00168       dist_queue.pop();
00169 
00170       if(current_cell->cx > 0){
00171         check_cell = current_cell - 1;
00172         if(!check_cell->path_mark){
00173           updatePathCell(current_cell, check_cell, dist_queue, costmap);
00174         }
00175       }
00176 
00177       if(current_cell->cx < last_col){
00178         check_cell = current_cell + 1;
00179         if(!check_cell->path_mark){
00180           updatePathCell(current_cell, check_cell, dist_queue, costmap);
00181         }
00182       }
00183 
00184       if(current_cell->cy > 0){
00185         check_cell = current_cell - size_x_;
00186         if(!check_cell->path_mark){
00187           updatePathCell(current_cell, check_cell, dist_queue, costmap);
00188         }
00189       }
00190 
00191       if(current_cell->cy < last_row){
00192         check_cell = current_cell + size_x_;
00193         if(!check_cell->path_mark){
00194           updatePathCell(current_cell, check_cell, dist_queue, costmap);
00195         }
00196       }
00197     }
00198   }
00199 
00200   void MapGrid::computeGoalDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
00201     MapCell* current_cell;
00202     MapCell* check_cell;
00203     unsigned int last_col = size_x_ - 1;
00204     unsigned int last_row = size_y_ - 1;
00205     while(!dist_queue.empty()){
00206       current_cell = dist_queue.front();
00207       current_cell->goal_mark = true;
00208       check_cell = current_cell;
00209       dist_queue.pop();
00210 
00211       if(current_cell->cx > 0){
00212         check_cell = current_cell - 1;
00213         if(!check_cell->goal_mark){
00214           updateGoalCell(current_cell, check_cell, dist_queue, costmap);
00215         }
00216       }
00217 
00218       if(current_cell->cx < last_col){
00219         check_cell = current_cell + 1;
00220         if(!check_cell->goal_mark){
00221           updateGoalCell(current_cell, check_cell, dist_queue, costmap);
00222         }
00223       }
00224 
00225       if(current_cell->cy > 0){
00226         check_cell = current_cell - size_x_;
00227         if(!check_cell->goal_mark){
00228           updateGoalCell(current_cell, check_cell, dist_queue, costmap);
00229         }
00230       }
00231 
00232       if(current_cell->cy < last_row){
00233         check_cell = current_cell + size_x_;
00234         if(!check_cell->goal_mark){
00235           updateGoalCell(current_cell, check_cell, dist_queue, costmap);
00236         }
00237       }
00238     }
00239   }
00240 
00241 
00242 };
map_grid.cpp
