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     double min_sq_resolution = resolution * resolution;
00143 
00144     for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
00145       double loop_x = global_plan_in[i].pose.position.x;
00146       double loop_y = global_plan_in[i].pose.position.y;
00147       double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
00148       if (sqdist > min_sq_resolution) {
00149         int steps = ceil((sqrt(sqdist)) / resolution);
00150         // add a points in-between
00151         double deltax = (loop_x - last_x) / steps;
00152         double deltay = (loop_y - last_y) / steps;
00153         // TODO: Interpolate orientation
00154         for (int j = 1; j < steps; ++j) {
00155           geometry_msgs::PoseStamped pose;
00156           pose.pose.position.x = last_x + j * deltax;
00157           pose.pose.position.y = last_y + j * deltay;
00158           pose.pose.position.z = global_plan_in[i].pose.position.z;
00159           pose.pose.orientation = global_plan_in[i].pose.orientation;
00160           pose.header = global_plan_in[i].header;
00161           global_plan_out.push_back(pose);
00162         }
00163       }
00164       global_plan_out.push_back(global_plan_in[i]);
00165       last_x = loop_x;
00166       last_y = loop_y;
00167     }
00168   }
00169 
00170   //update what map cells are considered path based on the global_plan
00171   void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
00172       const std::vector<geometry_msgs::PoseStamped>& global_plan) {
00173     sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
00174 
00175     bool started_path = false;
00176 
00177     queue<MapCell*> path_dist_queue;
00178 
00179     std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
00180     adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
00181     if (adjusted_global_plan.size() != global_plan.size()) {
00182       ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
00183     }
00184     unsigned int i;
00185     // put global path points into local map until we reach the border of the local map
00186     for (i = 0; i < adjusted_global_plan.size(); ++i) {
00187       double g_x = adjusted_global_plan[i].pose.position.x;
00188       double g_y = adjusted_global_plan[i].pose.position.y;
00189       unsigned int map_x, map_y;
00190       if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
00191         MapCell& current = getCell(map_x, map_y);
00192         current.target_dist = 0.0;
00193         current.target_mark = true;
00194         path_dist_queue.push(&current);
00195         started_path = true;
00196       } else if (started_path) {
00197           break;
00198       }
00199     }
00200     if (!started_path) {
00201       ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
00202           i, adjusted_global_plan.size(), global_plan.size());
00203       return;
00204     }
00205 
00206     computeTargetDistance(path_dist_queue, costmap);
00207   }
00208 
00209   //mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
00210   void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,
00211       const std::vector<geometry_msgs::PoseStamped>& global_plan) {
00212     sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
00213 
00214     int local_goal_x = -1;
00215     int local_goal_y = -1;
00216     bool started_path = false;
00217 
00218     std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
00219     adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
00220 
00221     // skip global path points until we reach the border of the local map
00222     for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
00223       double g_x = adjusted_global_plan[i].pose.position.x;
00224       double g_y = adjusted_global_plan[i].pose.position.y;
00225       unsigned int map_x, map_y;
00226       if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
00227         local_goal_x = map_x;
00228         local_goal_y = map_y;
00229         started_path = true;
00230       } else {
00231         if (started_path) {
00232           break;
00233         }// else we might have a non pruned path, so we just continue
00234       }
00235     }
00236     if (!started_path) {
00237       ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
00238       return;
00239     }
00240 
00241     queue<MapCell*> path_dist_queue;
00242     if (local_goal_x >= 0 && local_goal_y >= 0) {
00243       MapCell& current = getCell(local_goal_x, local_goal_y);
00244       costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
00245       current.target_dist = 0.0;
00246       current.target_mark = true;
00247       path_dist_queue.push(&current);
00248     }
00249 
00250     computeTargetDistance(path_dist_queue, costmap);
00251   }
00252 
00253 
00254 
00255   void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
00256     MapCell* current_cell;
00257     MapCell* check_cell;
00258     unsigned int last_col = size_x_ - 1;
00259     unsigned int last_row = size_y_ - 1;
00260     while(!dist_queue.empty()){
00261       current_cell = dist_queue.front();
00262 
00263 
00264       dist_queue.pop();
00265 
00266       if(current_cell->cx > 0){
00267         check_cell = current_cell - 1;
00268         if(!check_cell->target_mark){
00269           //mark the cell as visisted
00270           check_cell->target_mark = true;
00271           if(updatePathCell(current_cell, check_cell, costmap)) {
00272             dist_queue.push(check_cell);
00273           }
00274         }
00275       }
00276 
00277       if(current_cell->cx < last_col){
00278         check_cell = current_cell + 1;
00279         if(!check_cell->target_mark){
00280           check_cell->target_mark = true;
00281           if(updatePathCell(current_cell, check_cell, costmap)) {
00282             dist_queue.push(check_cell);
00283           }
00284         }
00285       }
00286 
00287       if(current_cell->cy > 0){
00288         check_cell = current_cell - size_x_;
00289         if(!check_cell->target_mark){
00290           check_cell->target_mark = true;
00291           if(updatePathCell(current_cell, check_cell, costmap)) {
00292             dist_queue.push(check_cell);
00293           }
00294         }
00295       }
00296 
00297       if(current_cell->cy < last_row){
00298         check_cell = current_cell + size_x_;
00299         if(!check_cell->target_mark){
00300           check_cell->target_mark = true;
00301           if(updatePathCell(current_cell, check_cell, costmap)) {
00302             dist_queue.push(check_cell);
00303           }
00304         }
00305       }
00306     }
00307   }
00308 
00309 };


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:30