inflation_layer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  * David V. Lu!!
37  *********************************************************************/
38 #include <algorithm>
41 #include <costmap_2d/footprint.h>
42 #include <boost/thread.hpp>
44 
46 
50 
51 namespace costmap_2d
52 {
53 
54 InflationLayer::InflationLayer()
55  : inflation_radius_(0)
56  , weight_(0)
57  , inflate_unknown_(false)
58  , cell_inflation_radius_(0)
59  , cached_cell_inflation_radius_(0)
60  , dsrv_(NULL)
61  , seen_(NULL)
62  , cached_costs_(NULL)
63  , cached_distances_(NULL)
64  , last_min_x_(-std::numeric_limits<float>::max())
65  , last_min_y_(-std::numeric_limits<float>::max())
66  , last_max_x_(std::numeric_limits<float>::max())
67  , last_max_y_(std::numeric_limits<float>::max())
68 {
69  inflation_access_ = new boost::recursive_mutex();
70 }
71 
73 {
74  {
75  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
76  ros::NodeHandle nh("~/" + name_), g_nh;
77  current_ = true;
78  if (seen_)
79  delete[] seen_;
80  seen_ = NULL;
81  seen_size_ = 0;
82  need_reinflation_ = false;
83 
84  dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(
85  &InflationLayer::reconfigureCB, this, _1, _2);
86 
87  if (dsrv_ != NULL){
88  dsrv_->clearCallback();
89  dsrv_->setCallback(cb);
90  }
91  else
92  {
93  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(ros::NodeHandle("~/" + name_));
94  dsrv_->setCallback(cb);
95  }
96  }
97 
98  matchSize();
99 }
100 
101 void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
102 {
103  setInflationParameters(config.inflation_radius, config.cost_scaling_factor);
104 
105  if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown) {
106  enabled_ = config.enabled;
107  inflate_unknown_ = config.inflate_unknown;
108  need_reinflation_ = true;
109  }
110 }
111 
113 {
114  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
116  resolution_ = costmap->getResolution();
118  computeCaches();
119 
120  unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
121  if (seen_)
122  delete[] seen_;
123  seen_size_ = size_x * size_y;
124  seen_ = new bool[seen_size_];
125 }
126 
127 void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
128  double* min_y, double* max_x, double* max_y)
129 {
130  if (need_reinflation_)
131  {
132  last_min_x_ = *min_x;
133  last_min_y_ = *min_y;
134  last_max_x_ = *max_x;
135  last_max_y_ = *max_y;
136  // For some reason when I make these -<double>::max() it does not
137  // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
138  // -<float>::max() instead.
139  *min_x = -std::numeric_limits<float>::max();
140  *min_y = -std::numeric_limits<float>::max();
141  *max_x = std::numeric_limits<float>::max();
142  *max_y = std::numeric_limits<float>::max();
143  need_reinflation_ = false;
144  }
145  else
146  {
147  double tmp_min_x = last_min_x_;
148  double tmp_min_y = last_min_y_;
149  double tmp_max_x = last_max_x_;
150  double tmp_max_y = last_max_y_;
151  last_min_x_ = *min_x;
152  last_min_y_ = *min_y;
153  last_max_x_ = *max_x;
154  last_max_y_ = *max_y;
155  *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
156  *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
157  *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
158  *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
159  }
160 }
161 
163 {
166  computeCaches();
167  need_reinflation_ = true;
168 
169  ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
170  " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
172 }
173 
174 void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
175 {
176  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
177  if (!enabled_ || (cell_inflation_radius_ == 0))
178  return;
179 
180  // make sure the inflation list is empty at the beginning of the cycle (should always be true)
181  ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
182 
183  unsigned char* master_array = master_grid.getCharMap();
184  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
185 
186  if (seen_ == NULL) {
187  ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
188  seen_size_ = size_x * size_y;
189  seen_ = new bool[seen_size_];
190  }
191  else if (seen_size_ != size_x * size_y)
192  {
193  ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
194  delete[] seen_;
195  seen_size_ = size_x * size_y;
196  seen_ = new bool[seen_size_];
197  }
198  memset(seen_, false, size_x * size_y * sizeof(bool));
199 
200  // We need to include in the inflation cells outside the bounding
201  // box min_i...max_j, by the amount cell_inflation_radius_. Cells
202  // up to that distance outside the box can still influence the costs
203  // stored in cells inside the box.
204  min_i -= cell_inflation_radius_;
205  min_j -= cell_inflation_radius_;
206  max_i += cell_inflation_radius_;
207  max_j += cell_inflation_radius_;
208 
209  min_i = std::max(0, min_i);
210  min_j = std::max(0, min_j);
211  max_i = std::min(int(size_x), max_i);
212  max_j = std::min(int(size_y), max_j);
213 
214  // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
215  // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
216 
217  // Start with lethal obstacles: by definition distance is 0.0
218  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
219  for (int j = min_j; j < max_j; j++)
220  {
221  for (int i = min_i; i < max_i; i++)
222  {
223  int index = master_grid.getIndex(i, j);
224  unsigned char cost = master_array[index];
225  if (cost == LETHAL_OBSTACLE)
226  {
227  obs_bin.push_back(CellData(index, i, j, i, j));
228  }
229  }
230  }
231 
232  // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
233  // can overtake previously inserted but farther away cells
234  std::map<double, std::vector<CellData> >::iterator bin;
235  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
236  {
237  for (int i = 0; i < bin->second.size(); ++i)
238  {
239  // process all cells at distance dist_bin.first
240  const CellData& cell = bin->second[i];
241 
242  unsigned int index = cell.index_;
243 
244  // ignore if already visited
245  if (seen_[index])
246  {
247  continue;
248  }
249 
250  seen_[index] = true;
251 
252  unsigned int mx = cell.x_;
253  unsigned int my = cell.y_;
254  unsigned int sx = cell.src_x_;
255  unsigned int sy = cell.src_y_;
256 
257  // assign the cost associated with the distance from an obstacle to the cell
258  unsigned char cost = costLookup(mx, my, sx, sy);
259  unsigned char old_cost = master_array[index];
260  if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
261  master_array[index] = cost;
262  else
263  master_array[index] = std::max(old_cost, cost);
264 
265  // attempt to put the neighbors of the current cell onto the inflation list
266  if (mx > 0)
267  enqueue(index - 1, mx - 1, my, sx, sy);
268  if (my > 0)
269  enqueue(index - size_x, mx, my - 1, sx, sy);
270  if (mx < size_x - 1)
271  enqueue(index + 1, mx + 1, my, sx, sy);
272  if (my < size_y - 1)
273  enqueue(index + size_x, mx, my + 1, sx, sy);
274  }
275  }
276 
277  inflation_cells_.clear();
278 }
279 
289 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
290  unsigned int src_x, unsigned int src_y)
291 {
292  if (!seen_[index])
293  {
294  // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
295  double distance = distanceLookup(mx, my, src_x, src_y);
296 
297  // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
298  if (distance > cell_inflation_radius_)
299  return;
300 
301  // push the cell data onto the inflation list and mark
302  inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
303  }
304 }
305 
307 {
308  if (cell_inflation_radius_ == 0)
309  return;
310 
311  // based on the inflation radius... compute distance and cost caches
313  {
314  deleteKernels();
315 
316  cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
317  cached_distances_ = new double*[cell_inflation_radius_ + 2];
318 
319  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
320  {
321  cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
322  cached_distances_[i] = new double[cell_inflation_radius_ + 2];
323  for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
324  {
325  cached_distances_[i][j] = hypot(i, j);
326  }
327  }
328 
330  }
331 
332  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
333  {
334  for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
335  {
337  }
338  }
339 }
340 
342 {
343  if (cached_distances_ != NULL)
344  {
345  for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
346  {
347  if (cached_distances_[i])
348  delete[] cached_distances_[i];
349  }
350  if (cached_distances_)
351  delete[] cached_distances_;
352  cached_distances_ = NULL;
353  }
354 
355  if (cached_costs_ != NULL)
356  {
357  for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
358  {
359  if (cached_costs_[i])
360  delete[] cached_costs_[i];
361  }
362  delete[] cached_costs_;
363  cached_costs_ = NULL;
364  }
365 }
366 
367 void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
368 {
369  if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
370  {
371  // Lock here so that reconfiguring the inflation radius doesn't cause segfaults
372  // when accessing the cached arrays
373  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
374 
375  inflation_radius_ = inflation_radius;
377  weight_ = cost_scaling_factor;
378  need_reinflation_ = true;
379  computeCaches();
380  }
381 }
382 
383 } // namespace costmap_2d
unsigned char computeCost(double distance) const
Given a distance, compute a cost.
LayeredCostmap * layered_costmap_
Definition: layer.h:122
virtual void onFootprintChanged()
LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot&#39;s footprint.
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
std::map< double, std::vector< CellData > > inflation_cells_
static const unsigned char FREE_SPACE
Definition: cost_values.h:45
#define ROS_WARN(...)
void setInflationParameters(double inflation_radius, double cost_scaling_factor)
Change the values of the inflation radius parameters.
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:187
std::string name_
Definition: layer.h:125
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization...
boost::recursive_mutex * inflation_access_
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
double getInscribedRadius()
The radius of a circle centered at the origin of the robot which is just within all points and edges ...
void enqueue(unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y)
Given an index of a cell in the costmap, place it into a list pending for obstacle inflation...
double distance(double x0, double y0, double x1, double y1)
Definition: costmap_math.h:58
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:44
#define ROS_ASSERT_MSG(cond,...)
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to up...
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
const std::vector< geometry_msgs::Point > & getFootprint()
Returns the latest footprint stored with setFootprint().
void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
Storage for cell information used during obstacle inflation.
bool enabled_
Currently this var is managed by subclasses. TODO: make this managed by this class and/or container c...
Definition: layer.h:124
unsigned char ** cached_costs_
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:426
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
unsigned int cached_cell_inflation_radius_
unsigned char costLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed costs.
double distanceLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed distances.
unsigned int cellDistance(double world_dist)
bool need_reinflation_
Indicates that the entire costmap should be reinflated next time around.
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
dynamic_reconfigure::Server< costmap_2d::InflationPluginConfig > * dsrv_
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:456
double max(double a, double b)
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.h:171
#define ROS_DEBUG(...)


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42