inflation.hpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Ifdefs
6 *****************************************************************************/
7 
8 #ifndef cost_map_core_INFLATION_HPP_
9 #define cost_map_core_INFLATION_HPP_
10 
11 /*****************************************************************************
12 ** Includes
13 *****************************************************************************/
14 
15 #include "../cost_map.hpp"
16 #include <limits>
17 #include <queue>
18 
19 /*****************************************************************************
20 ** Namespaces
21 *****************************************************************************/
22 
23 namespace cost_map {
24 
25 /*****************************************************************************
26 ** Inflation Function
27 *****************************************************************************/
28 
36 public:
38  virtual ~InflationComputer() {};
39 
45  virtual unsigned char operator()(const float &distance) const = 0;
46 };
47 
56 public:
57  ROSInflationComputer(const float& inscribed_radius, const float& weight);
58 
59  virtual ~ROSInflationComputer() {};
60 
66  virtual unsigned char operator()(const float &distance) const;
67 private:
68  float inscribed_radius_, weight_;
69 };
70 
71 /*****************************************************************************
72 ** Inflation
73 *****************************************************************************/
74 
75 class Inflate {
76 public:
77  Inflate() : cell_inflation_radius_(std::numeric_limits<unsigned int>::max())
78  {
79  };
80 
91  void operator()(const std::string layer_source,
92  const std::string layer_destination,
93  const float& inflation_radius,
94  const InflationComputer& inflation_computer,
96  );
97 
98 private:
99  struct CellData {
109  CellData(double d, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
110  distance_(d), x_(x), y_(y), src_x_(sx), src_y_(sy)
111  {
112  }
117  friend bool operator<(const CellData &a, const CellData &b) {
118  return a.distance_ > b.distance_;
119  }
120  double distance_;
121  unsigned int x_, y_;
122  unsigned int src_x_, src_y_;
123  };
124 
133  void enqueue(const cost_map::Matrix& data_source,
134  cost_map::Matrix& data_destination,
135  unsigned int mx, unsigned int my,
136  unsigned int src_x, unsigned int src_y
137  );
138 
147  double distanceLookup(int mx, int my, int src_x, int src_y);
148 
157  unsigned char costLookup(int mx, int my, int src_x, int src_y);
158  void computeCaches(const float& resolution, const InflationComputer& compute_cost);
159 
160  Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> seen_;
161  Eigen::MatrixXf cached_distances_;
163  std::priority_queue<CellData> inflation_queue_;
164  unsigned int cell_inflation_radius_;
165 };
166 
167 /*****************************************************************************
168 ** Deflation
169 *****************************************************************************/
170 
174 class Deflate {
175 public:
176  Deflate(const bool& do_not_strip_inscribed_region=false);
177 
186  void operator()(const std::string layer_source,
187  const std::string layer_destination,
189  );
190 
191 private:
193 };
194 
195 /*****************************************************************************
196 ** Trailers
197 *****************************************************************************/
198 
199 } // namespace cost_map_core
200 
201 #endif /* cost_map_core_INFLATION_HPP_ */
Function which can compute costs for the inflation layer.
Definition: inflation.hpp:35
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: common.hpp:30
cost_map::Matrix cached_costs_
Definition: inflation.hpp:162
unsigned int cell_inflation_radius_
Definition: inflation.hpp:164
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic > seen_
Definition: inflation.hpp:160
Functor to strip the inflation layer from a cost map.
Definition: inflation.hpp:174
std::priority_queue< CellData > inflation_queue_
Definition: inflation.hpp:163
bool do_not_strip_inscribed_region
Definition: inflation.hpp:192
Function which can compute costs for the inflation layer.
Definition: inflation.hpp:55
friend bool operator<(const CellData &a, const CellData &b)
Provide an ordering between CellData objects in the priority queue.
Definition: inflation.hpp:117
Eigen::MatrixXf cached_distances_
Definition: inflation.hpp:161
virtual unsigned char operator()(const float &distance) const =0
Given a distance, compute a cost.
CellData(double d, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
Constructor for CellData objects.
Definition: inflation.hpp:109


cost_map_core
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:41