map_grid.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
35 #include <costmap_2d/cost_values.h>
36 using namespace std;
37 
38 namespace base_local_planner{
39 
40  MapGrid::MapGrid()
41  : size_x_(0), size_y_(0)
42  {
43  }
44 
45  MapGrid::MapGrid(unsigned int size_x, unsigned int size_y)
46  : size_x_(size_x), size_y_(size_y)
47  {
48  commonInit();
49  }
50 
52  size_y_ = mg.size_y_;
53  size_x_ = mg.size_x_;
54  map_ = mg.map_;
55  }
56 
58  //don't allow construction of zero size grid
59  ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
60 
61  map_.resize(size_y_ * size_x_);
62 
63  //make each cell aware of its location in the grid
64  for(unsigned int i = 0; i < size_y_; ++i){
65  for(unsigned int j = 0; j < size_x_; ++j){
66  unsigned int id = size_x_ * i + j;
67  map_[id].cx = j;
68  map_[id].cy = i;
69  }
70  }
71  }
72 
73  size_t MapGrid::getIndex(int x, int y){
74  return size_x_ * y + x;
75  }
76 
78  size_y_ = mg.size_y_;
79  size_x_ = mg.size_x_;
80  map_ = mg.map_;
81  return *this;
82  }
83 
84  void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y){
85  if(map_.size() != size_x * size_y)
86  map_.resize(size_x * size_y);
87 
88  if(size_x_ != size_x || size_y_ != size_y){
89  size_x_ = size_x;
90  size_y_ = size_y;
91 
92  for(unsigned int i = 0; i < size_y_; ++i){
93  for(unsigned int j = 0; j < size_x_; ++j){
94  int index = size_x_ * i + j;
95  map_[index].cx = j;
96  map_[index].cy = i;
97  }
98  }
99  }
100  }
101 
102 
103  inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
104  const costmap_2d::Costmap2D& costmap){
105 
106  //if the cell is an obstacle set the max path distance
107  unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
108  if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
109  (cost == costmap_2d::LETHAL_OBSTACLE ||
111  cost == costmap_2d::NO_INFORMATION)){
112  check_cell->target_dist = obstacleCosts();
113  return false;
114  }
115 
116  double new_target_dist = current_cell->target_dist + 1;
117  if (new_target_dist < check_cell->target_dist) {
118  check_cell->target_dist = new_target_dist;
119  }
120  return true;
121  }
122 
123 
124  //reset the path_dist and goal_dist fields for all cells
126  for(unsigned int i = 0; i < map_.size(); ++i) {
127  map_[i].target_dist = unreachableCellCosts();
128  map_[i].target_mark = false;
129  map_[i].within_robot = false;
130  }
131  }
132 
133  void MapGrid::adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
134  std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution) {
135  if (global_plan_in.size() == 0) {
136  return;
137  }
138  double last_x = global_plan_in[0].pose.position.x;
139  double last_y = global_plan_in[0].pose.position.y;
140  global_plan_out.push_back(global_plan_in[0]);
141 
142  double min_sq_resolution = resolution * resolution;
143 
144  for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
145  double loop_x = global_plan_in[i].pose.position.x;
146  double loop_y = global_plan_in[i].pose.position.y;
147  double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
148  if (sqdist > min_sq_resolution) {
149  int steps = ceil((sqrt(sqdist)) / resolution);
150  // add a points in-between
151  double deltax = (loop_x - last_x) / steps;
152  double deltay = (loop_y - last_y) / steps;
153  // TODO: Interpolate orientation
154  for (int j = 1; j < steps; ++j) {
155  geometry_msgs::PoseStamped pose;
156  pose.pose.position.x = last_x + j * deltax;
157  pose.pose.position.y = last_y + j * deltay;
158  pose.pose.position.z = global_plan_in[i].pose.position.z;
159  pose.pose.orientation = global_plan_in[i].pose.orientation;
160  pose.header = global_plan_in[i].header;
161  global_plan_out.push_back(pose);
162  }
163  }
164  global_plan_out.push_back(global_plan_in[i]);
165  last_x = loop_x;
166  last_y = loop_y;
167  }
168  }
169 
170  //update what map cells are considered path based on the global_plan
172  const std::vector<geometry_msgs::PoseStamped>& global_plan) {
173  sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
174 
175  bool started_path = false;
176 
177  queue<MapCell*> path_dist_queue;
178 
179  std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
180  adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
181  if (adjusted_global_plan.size() != global_plan.size()) {
182  ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
183  }
184  unsigned int i;
185  // put global path points into local map until we reach the border of the local map
186  for (i = 0; i < adjusted_global_plan.size(); ++i) {
187  double g_x = adjusted_global_plan[i].pose.position.x;
188  double g_y = adjusted_global_plan[i].pose.position.y;
189  unsigned int map_x, map_y;
190  if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
191  MapCell& current = getCell(map_x, map_y);
192  current.target_dist = 0.0;
193  current.target_mark = true;
194  path_dist_queue.push(&current);
195  started_path = true;
196  } else if (started_path) {
197  break;
198  }
199  }
200  if (!started_path) {
201  ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
202  i, adjusted_global_plan.size(), global_plan.size());
203  return;
204  }
205 
206  computeTargetDistance(path_dist_queue, costmap);
207  }
208 
209  //mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
211  const std::vector<geometry_msgs::PoseStamped>& global_plan) {
212  sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
213 
214  int local_goal_x = -1;
215  int local_goal_y = -1;
216  bool started_path = false;
217 
218  std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
219  adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
220 
221  // skip global path points until we reach the border of the local map
222  for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
223  double g_x = adjusted_global_plan[i].pose.position.x;
224  double g_y = adjusted_global_plan[i].pose.position.y;
225  unsigned int map_x, map_y;
226  if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
227  local_goal_x = map_x;
228  local_goal_y = map_y;
229  started_path = true;
230  } else {
231  if (started_path) {
232  break;
233  }// else we might have a non pruned path, so we just continue
234  }
235  }
236  if (!started_path) {
237  ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
238  return;
239  }
240 
241  queue<MapCell*> path_dist_queue;
242  if (local_goal_x >= 0 && local_goal_y >= 0) {
243  MapCell& current = getCell(local_goal_x, local_goal_y);
244  costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
245  current.target_dist = 0.0;
246  current.target_mark = true;
247  path_dist_queue.push(&current);
248  }
249 
250  computeTargetDistance(path_dist_queue, costmap);
251  }
252 
253 
254 
255  void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
256  MapCell* current_cell;
257  MapCell* check_cell;
258  unsigned int last_col = size_x_ - 1;
259  unsigned int last_row = size_y_ - 1;
260  while(!dist_queue.empty()){
261  current_cell = dist_queue.front();
262 
263 
264  dist_queue.pop();
265 
266  if(current_cell->cx > 0){
267  check_cell = current_cell - 1;
268  if(!check_cell->target_mark){
269  //mark the cell as visisted
270  check_cell->target_mark = true;
271  if(updatePathCell(current_cell, check_cell, costmap)) {
272  dist_queue.push(check_cell);
273  }
274  }
275  }
276 
277  if(current_cell->cx < last_col){
278  check_cell = current_cell + 1;
279  if(!check_cell->target_mark){
280  check_cell->target_mark = true;
281  if(updatePathCell(current_cell, check_cell, costmap)) {
282  dist_queue.push(check_cell);
283  }
284  }
285  }
286 
287  if(current_cell->cy > 0){
288  check_cell = current_cell - size_x_;
289  if(!check_cell->target_mark){
290  check_cell->target_mark = true;
291  if(updatePathCell(current_cell, check_cell, costmap)) {
292  dist_queue.push(check_cell);
293  }
294  }
295  }
296 
297  if(current_cell->cy < last_row){
298  check_cell = current_cell + size_x_;
299  if(!check_cell->target_mark){
300  check_cell->target_mark = true;
301  if(updatePathCell(current_cell, check_cell, costmap)) {
302  dist_queue.push(check_cell);
303  }
304  }
305  }
306  }
307  }
308 
309 };
unsigned int cy
Cell index in the grid map.
Definition: map_cell.h:57
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
void setTargetCells(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cells are considered path based on the global plan.
Definition: map_grid.cpp:171
void computeTargetDistance(std::queue< MapCell * > &dist_queue, const costmap_2d::Costmap2D &costmap)
Compute the distance from each cell in the local map grid to the planned path.
Definition: map_grid.cpp:255
bool within_robot
Mark for cells within the robot footprint.
Definition: map_cell.h:63
unsigned int size_y_
The dimensions of the grid.
Definition: map_grid.h:191
double goal_y_
The goal distance was last computed from.
Definition: map_grid.h:189
bool updatePathCell(MapCell *current_cell, MapCell *check_cell, const costmap_2d::Costmap2D &costmap)
Used to update the distance of a cell in path distance computation.
Definition: map_grid.cpp:103
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:52
unsigned char getCost(unsigned int mx, unsigned int my) const
static void adjustPlanResolution(const std::vector< geometry_msgs::PoseStamped > &global_plan_in, std::vector< geometry_msgs::PoseStamped > &global_plan_out, double resolution)
Definition: map_grid.cpp:133
void commonInit()
Utility to share initialization code across constructors.
Definition: map_grid.cpp:57
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
size_t getIndex(int x, int y)
Returns a 1D index into the MapCell array for a 2D index.
Definition: map_grid.cpp:73
MapGrid()
Creates a 0x0 map by default.
Definition: map_grid.cpp:40
void sizeCheck(unsigned int size_x, unsigned int size_y)
check if we need to resize
Definition: map_grid.cpp:84
MapGrid & operator=(const MapGrid &mg)
Assignment operator for a MapGrid.
Definition: map_grid.cpp:77
std::vector< MapCell > map_
Storage for the MapCells.
Definition: map_grid.h:195
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
unsigned int getSizeInCellsY() const
bool target_mark
Marks for computing path/goal distances.
Definition: map_cell.h:61
double target_dist
Distance to planner&#39;s path.
Definition: map_cell.h:59
unsigned int getSizeInCellsX() const
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
Stores path distance and goal distance information used for scoring trajectories. ...
Definition: map_cell.h:44
double getResolution() const
#define ROS_ASSERT(cond)
void setLocalGoal(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cell is considered the next local goal.
Definition: map_grid.cpp:210
MapCell & getCell(unsigned int x, unsigned int y)
Definition: map_grid.h:87
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125
#define ROS_ERROR(...)
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
#define ROS_DEBUG(...)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49