inflation_layer.h
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 #ifndef COSTMAP_2D_INFLATION_LAYER_H_
39 #define COSTMAP_2D_INFLATION_LAYER_H_
40 
41 #include <ros/ros.h>
42 #include <costmap_2d/layer.h>
44 #include <costmap_2d/InflationPluginConfig.h>
45 #include <dynamic_reconfigure/server.h>
46 #include <boost/thread.hpp>
47 
48 namespace costmap_2d
49 {
54 class CellData
55 {
56 public:
66  CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
67  index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
68  {
69  }
70  unsigned int index_;
71  unsigned int x_, y_;
72  unsigned int src_x_, src_y_;
73 };
74 
75 class InflationLayer : public Layer
76 {
77 public:
79 
80  virtual ~InflationLayer()
81  {
82  deleteKernels();
83  if (dsrv_)
84  delete dsrv_;
85  }
86 
87  virtual void onInitialize();
88  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
89  double* max_x, double* max_y);
90  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
91  virtual bool isDiscretized()
92  {
93  return true;
94  }
95  virtual void matchSize();
96 
97  virtual void reset() { onInitialize(); }
98 
102  inline unsigned char computeCost(double distance) const
103  {
104  unsigned char cost = 0;
105  if (distance == 0)
106  cost = LETHAL_OBSTACLE;
107  else if (distance * resolution_ <= inscribed_radius_)
109  else
110  {
111  // make sure cost falls off by Euclidean distance
112  double euclidean_distance = distance * resolution_;
113  double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
114  cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
115  }
116  return cost;
117  }
118 
124  void setInflationParameters(double inflation_radius, double cost_scaling_factor);
125 
126 protected:
127  virtual void onFootprintChanged();
128  boost::recursive_mutex* inflation_access_;
129 
130 private:
139  inline double distanceLookup(int mx, int my, int src_x, int src_y)
140  {
141  unsigned int dx = abs(mx - src_x);
142  unsigned int dy = abs(my - src_y);
143  return cached_distances_[dx][dy];
144  }
145 
154  inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
155  {
156  unsigned int dx = abs(mx - src_x);
157  unsigned int dy = abs(my - src_y);
158  return cached_costs_[dx][dy];
159  }
160 
161  void computeCaches();
162  void deleteKernels();
163  void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
164 
165  unsigned int cellDistance(double world_dist)
166  {
167  return layered_costmap_->getCostmap()->cellDistance(world_dist);
168  }
169 
170  inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
171  unsigned int src_x, unsigned int src_y);
172 
173  double inflation_radius_, inscribed_radius_, weight_;
177  std::map<double, std::vector<CellData> > inflation_cells_;
178 
179  double resolution_;
180 
181  bool* seen_;
183 
184  unsigned char** cached_costs_;
186  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
187 
188  dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;
189  void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);
190 
192 };
193 
194 } // namespace costmap_2d
195 
196 #endif // COSTMAP_2D_INFLATION_LAYER_H_
unsigned char computeCost(double distance) const
Given a distance, compute a cost.
std::map< double, std::vector< CellData > > inflation_cells_
boost::recursive_mutex * inflation_access_
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
CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
Constructor for a CellData objects.
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Storage for cell information used during obstacle inflation.
unsigned char ** cached_costs_
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
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_


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