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 
55  : resolution_(0)
56  , inflation_radius_(0)
57  , inscribed_radius_(0)
58  , weight_(0)
59  , inflate_unknown_(false)
60  , cell_inflation_radius_(0)
61  , cached_cell_inflation_radius_(0)
62  , dsrv_(NULL)
63  , seen_(NULL)
64  , cached_costs_(NULL)
65  , cached_distances_(NULL)
66  , last_min_x_(-std::numeric_limits<float>::max())
67  , last_min_y_(-std::numeric_limits<float>::max())
68  , last_max_x_(std::numeric_limits<float>::max())
69  , last_max_y_(std::numeric_limits<float>::max())
70 {
71  inflation_access_ = new boost::recursive_mutex();
72 }
73 
75 {
76  {
77  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
78  ros::NodeHandle nh("~/" + name_), g_nh;
79  current_ = true;
80  if (seen_)
81  delete[] seen_;
82  seen_ = NULL;
83  seen_size_ = 0;
84  need_reinflation_ = false;
85 
86  dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb =
87  [this](auto& config, auto level){ reconfigureCB(config, level); };
88 
89  if (dsrv_ != NULL){
90  dsrv_->clearCallback();
91  dsrv_->setCallback(cb);
92  }
93  else
94  {
95  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(ros::NodeHandle("~/" + name_));
96  dsrv_->setCallback(cb);
97  }
98  }
99 
100  matchSize();
101 }
102 
103 void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
104 {
105  setInflationParameters(config.inflation_radius, config.cost_scaling_factor);
106 
107  if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown) {
108  enabled_ = config.enabled;
109  inflate_unknown_ = config.inflate_unknown;
110  need_reinflation_ = true;
111  }
112 }
113 
115 {
116  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
118  resolution_ = costmap->getResolution();
120  computeCaches();
121 
122  unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
123  if (seen_)
124  delete[] seen_;
125  seen_size_ = size_x * size_y;
126  seen_ = new bool[seen_size_];
127 }
128 
129 void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
130  double* min_y, double* max_x, double* max_y)
131 {
132  if (need_reinflation_)
133  {
134  last_min_x_ = *min_x;
135  last_min_y_ = *min_y;
136  last_max_x_ = *max_x;
137  last_max_y_ = *max_y;
138  // For some reason when I make these -<double>::max() it does not
139  // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
140  // -<float>::max() instead.
141  *min_x = -std::numeric_limits<float>::max();
142  *min_y = -std::numeric_limits<float>::max();
143  *max_x = std::numeric_limits<float>::max();
144  *max_y = std::numeric_limits<float>::max();
145  need_reinflation_ = false;
146  }
147  else
148  {
149  double tmp_min_x = last_min_x_;
150  double tmp_min_y = last_min_y_;
151  double tmp_max_x = last_max_x_;
152  double tmp_max_y = last_max_y_;
153  last_min_x_ = *min_x;
154  last_min_y_ = *min_y;
155  last_max_x_ = *max_x;
156  last_max_y_ = *max_y;
157  *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
158  *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
159  *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
160  *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
161  }
162 }
163 
165 {
168  computeCaches();
169  need_reinflation_ = true;
170 
171  ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
172  " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
174 }
175 
176 void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
177 {
178  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
179  if (cell_inflation_radius_ == 0)
180  return;
181 
182  // make sure the inflation list is empty at the beginning of the cycle (should always be true)
183  ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
184 
185  unsigned char* master_array = master_grid.getCharMap();
186  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
187 
188  if (seen_ == NULL) {
189  ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
190  seen_size_ = size_x * size_y;
191  seen_ = new bool[seen_size_];
192  }
193  else if (seen_size_ != size_x * size_y)
194  {
195  ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
196  delete[] seen_;
197  seen_size_ = size_x * size_y;
198  seen_ = new bool[seen_size_];
199  }
200  memset(seen_, false, size_x * size_y * sizeof(bool));
201 
202  // We need to include in the inflation cells outside the bounding
203  // box min_i...max_j, by the amount cell_inflation_radius_. Cells
204  // up to that distance outside the box can still influence the costs
205  // stored in cells inside the box.
206  min_i -= cell_inflation_radius_;
207  min_j -= cell_inflation_radius_;
208  max_i += cell_inflation_radius_;
209  max_j += cell_inflation_radius_;
210 
211  min_i = std::max(0, min_i);
212  min_j = std::max(0, min_j);
213  max_i = std::min(int(size_x), max_i);
214  max_j = std::min(int(size_y), max_j);
215 
216  // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
217  // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
218 
219  // Start with lethal obstacles: by definition distance is 0.0
220  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
221  for (int j = min_j; j < max_j; j++)
222  {
223  for (int i = min_i; i < max_i; i++)
224  {
225  int index = master_grid.getIndex(i, j);
226  unsigned char cost = master_array[index];
227  if (cost == LETHAL_OBSTACLE)
228  {
229  obs_bin.push_back(CellData(index, i, j, i, j));
230  }
231  }
232  }
233 
234  // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
235  // can overtake previously inserted but farther away cells
236  std::map<double, std::vector<CellData> >::iterator bin;
237  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
238  {
239  for (int i = 0; i < bin->second.size(); ++i)
240  {
241  // process all cells at distance dist_bin.first
242  const CellData& cell = bin->second[i];
243 
244  unsigned int index = cell.index_;
245 
246  // ignore if already visited
247  if (seen_[index])
248  {
249  continue;
250  }
251 
252  seen_[index] = true;
253 
254  unsigned int mx = cell.x_;
255  unsigned int my = cell.y_;
256  unsigned int sx = cell.src_x_;
257  unsigned int sy = cell.src_y_;
258 
259  // assign the cost associated with the distance from an obstacle to the cell
260  unsigned char cost = costLookup(mx, my, sx, sy);
261  unsigned char old_cost = master_array[index];
262  if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
263  master_array[index] = cost;
264  else
265  master_array[index] = std::max(old_cost, cost);
266 
267  // attempt to put the neighbors of the current cell onto the inflation list
268  if (mx > 0)
269  enqueue(index - 1, mx - 1, my, sx, sy);
270  if (my > 0)
271  enqueue(index - size_x, mx, my - 1, sx, sy);
272  if (mx < size_x - 1)
273  enqueue(index + 1, mx + 1, my, sx, sy);
274  if (my < size_y - 1)
275  enqueue(index + size_x, mx, my + 1, sx, sy);
276  }
277  }
278 
279  inflation_cells_.clear();
280 }
281 
291 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
292  unsigned int src_x, unsigned int src_y)
293 {
294  if (!seen_[index])
295  {
296  // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
297  double distance = distanceLookup(mx, my, src_x, src_y);
298 
299  // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
301  return;
302 
303  // push the cell data onto the inflation list and mark
304  inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
305  }
306 }
307 
309 {
310  if (cell_inflation_radius_ == 0)
311  return;
312 
313  // based on the inflation radius... compute distance and cost caches
315  {
316  deleteKernels();
317 
318  cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
319  cached_distances_ = new double*[cell_inflation_radius_ + 2];
320 
321  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
322  {
323  cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
324  cached_distances_[i] = new double[cell_inflation_radius_ + 2];
325  for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
326  {
327  cached_distances_[i][j] = hypot(i, j);
328  }
329  }
330 
332  }
333 
334  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
335  {
336  for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
337  {
339  }
340  }
341 }
342 
344 {
345  if (cached_distances_ != NULL)
346  {
347  for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
348  {
349  if (cached_distances_[i])
350  delete[] cached_distances_[i];
351  }
352  if (cached_distances_)
353  delete[] cached_distances_;
354  cached_distances_ = NULL;
355  }
356 
357  if (cached_costs_ != NULL)
358  {
359  for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
360  {
361  if (cached_costs_[i])
362  delete[] cached_costs_[i];
363  }
364  delete[] cached_costs_;
365  cached_costs_ = NULL;
366  }
367 }
368 
369 void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
370 {
371  if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
372  {
373  // Lock here so that reconfiguring the inflation radius doesn't cause segfaults
374  // when accessing the cached arrays
375  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
376 
377  inflation_radius_ = inflation_radius;
379  weight_ = cost_scaling_factor;
380  need_reinflation_ = true;
381  computeCaches();
382  }
383 }
384 
385 } // namespace costmap_2d
costmap_2d::InflationLayer::inflation_access_
boost::recursive_mutex * inflation_access_
Definition: inflation_layer.h:166
costmap_2d::InflationLayer::inflation_cells_
std::map< double, std::vector< CellData > > inflation_cells_
Definition: inflation_layer.h:219
costmap_2d::InflationLayer::cellDistance
unsigned int cellDistance(double world_dist)
Definition: inflation_layer.h:209
costmap_2d::InflationLayer::dsrv_
dynamic_reconfigure::Server< costmap_2d::InflationPluginConfig > * dsrv_
Definition: inflation_layer.h:228
costmap_2d::InflationLayer::distanceLookup
double distanceLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed distances.
Definition: inflation_layer.h:183
costmap_2d::InflationLayer::inflate_unknown_
bool inflate_unknown_
Definition: inflation_layer.h:172
costmap_2d::Layer::enabled_
bool enabled_
Definition: layer.h:176
costmap_2d::InflationLayer::cached_costs_
unsigned char ** cached_costs_
Definition: inflation_layer.h:224
costmap_2d::InflationLayer::inscribed_radius_
double inscribed_radius_
Definition: inflation_layer.h:170
costmap_2d::InflationLayer::costLookup
unsigned char costLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed costs.
Definition: inflation_layer.h:198
costmap_2d::INSCRIBED_INFLATED_OBSTACLE
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:79
costmap_2d::InflationLayer::onInitialize
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization.
Definition: inflation_layer.cpp:74
costmap_2d::CellData::src_y_
unsigned int src_y_
Definition: inflation_layer.h:108
costmap_2d::InflationLayer::inflation_radius_
double inflation_radius_
Definition: inflation_layer.h:169
costmap_2d::NO_INFORMATION
static const unsigned char NO_INFORMATION
Definition: cost_values.h:77
costmap_2d::InflationLayer::updateCosts
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().
Definition: inflation_layer.cpp:176
costmap_2d::CellData
Storage for cell information used during obstacle inflation.
Definition: inflation_layer.h:90
costmap_2d::InflationLayer::computeCost
virtual unsigned char computeCost(double distance) const
Given a distance, compute a cost.
Definition: inflation_layer.h:140
costmap_2d::CellData::src_x_
unsigned int src_x_
Definition: inflation_layer.h:108
costmap_2d::Costmap2D
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:96
costmap_2d::Costmap2D::getCharMap
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:187
costmap_2d::Layer::name_
std::string name_
Definition: layer.h:177
costmap_2d::Layer::current_
bool current_
Definition: layer.h:175
costmap_2d::LayeredCostmap::getInscribedRadius
double getInscribedRadius()
The radius of a circle centered at the origin of the robot which is just within all points and edges ...
Definition: layered_costmap.h:191
costmap_2d::Layer::layered_costmap_
LayeredCostmap * layered_costmap_
Definition: layer.h:174
costmap_2d::InflationLayer::last_max_x_
double last_max_x_
Definition: inflation_layer.h:226
costmap_2d::InflationLayer::cached_distances_
double ** cached_distances_
Definition: inflation_layer.h:225
costmap_2d::CellData::index_
unsigned int index_
Definition: inflation_layer.h:106
costmap_2d::InflationLayer
Definition: inflation_layer.h:111
costmap_2d::InflationLayer::need_reinflation_
bool need_reinflation_
Indicates that the entire costmap should be reinflated next time around.
Definition: inflation_layer.h:231
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ROS_DEBUG
#define ROS_DEBUG(...)
costmap_2d::InflationLayer::seen_size_
int seen_size_
Definition: inflation_layer.h:222
costmap_2d::InflationLayer::reconfigureCB
void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
Definition: inflation_layer.cpp:103
costmap_2d::InflationLayer::onFootprintChanged
virtual void onFootprintChanged()
LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint())....
Definition: inflation_layer.cpp:164
costmap_2d::InflationLayer::cached_cell_inflation_radius_
unsigned int cached_cell_inflation_radius_
Definition: inflation_layer.h:218
costmap_2d::InflationLayer::setInflationParameters
void setInflationParameters(double inflation_radius, double cost_scaling_factor)
Change the values of the inflation radius parameters.
Definition: inflation_layer.cpp:369
costmap_2d::InflationLayer::last_max_y_
double last_max_y_
Definition: inflation_layer.h:226
costmap_2d::Costmap2D::getSizeInCellsY
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:435
ROS_ASSERT_MSG
#define ROS_ASSERT_MSG(cond,...)
costmap_2d::LayeredCostmap::getFootprint
const std::vector< geometry_msgs::Point > & getFootprint()
Returns the latest footprint stored with setFootprint().
Definition: layered_costmap.h:177
inflation_layer.h
ROS_WARN
#define ROS_WARN(...)
costmap_2d::LETHAL_OBSTACLE
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:78
distance
double distance(double x0, double y0, double x1, double y1)
Definition: costmap_math.h:58
costmap_2d::InflationLayer::resolution_
double resolution_
Definition: inflation_layer.h:168
costmap_math.h
costmap_2d::InflationLayer::enqueue
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.
Definition: inflation_layer.cpp:291
costmap_2d::InflationLayer::InflationLayer
InflationLayer()
Definition: inflation_layer.cpp:54
costmap_2d::InflationLayer::last_min_x_
double last_min_x_
Definition: inflation_layer.h:226
costmap_2d::InflationLayer::last_min_y_
double last_min_y_
Definition: inflation_layer.h:226
costmap_2d::InflationLayer::cell_inflation_radius_
unsigned int cell_inflation_radius_
Definition: inflation_layer.h:217
footprint.h
costmap_2d::CellData::x_
unsigned int x_
Definition: inflation_layer.h:107
costmap_2d::InflationLayer::seen_
bool * seen_
Definition: inflation_layer.h:221
std
costmap_2d::InflationLayer::updateBounds
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...
Definition: inflation_layer.cpp:129
costmap_2d::InflationLayer::computeCaches
void computeCaches()
Definition: inflation_layer.cpp:308
costmap_2d::CellData::y_
unsigned int y_
Definition: inflation_layer.h:107
class_list_macros.hpp
costmap_2d::InflationLayer::weight_
double weight_
Definition: inflation_layer.h:171
costmap_2d::Costmap2D::getResolution
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:460
costmap_2d::Layer
Definition: layer.h:84
costmap_2d::Costmap2D::getIndex
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.h:207
costmap_2d::LayeredCostmap::getCostmap
Costmap2D * getCostmap()
Definition: layered_costmap.h:128
costmap_2d::FREE_SPACE
static const unsigned char FREE_SPACE
Definition: cost_values.h:80
costmap_2d::InflationLayer::deleteKernels
void deleteKernels()
Definition: inflation_layer.cpp:343
costmap_2d::Costmap2D::getSizeInCellsX
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
costmap_2d
Definition: array_parser.h:37
costmap_2d::InflationLayer::matchSize
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
Definition: inflation_layer.cpp:114
ros::NodeHandle


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:17