inflation.hpp
Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Ifdefs
00006 *****************************************************************************/
00007 
00008 #ifndef cost_map_core_INFLATION_HPP_
00009 #define cost_map_core_INFLATION_HPP_
00010 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include "../cost_map.hpp"
00016 #include <limits>
00017 #include <queue>
00018 
00019 /*****************************************************************************
00020 ** Namespaces
00021 *****************************************************************************/
00022 
00023 namespace cost_map {
00024 
00025 /*****************************************************************************
00026 ** Inflation Function
00027 *****************************************************************************/
00028 
00035 class InflationComputer {
00036 public:
00037   InflationComputer() {};
00038   virtual ~InflationComputer() {};
00039 
00045   virtual unsigned char operator()(const float &distance) const = 0;
00046 };
00047 
00055 class ROSInflationComputer : public InflationComputer {
00056 public:
00057   ROSInflationComputer(const float& inscribed_radius, const float& weight);
00058 
00059   virtual ~ROSInflationComputer() {};
00060 
00066   virtual unsigned char operator()(const float &distance) const;
00067 private:
00068   float inscribed_radius_, weight_;
00069 };
00070 
00071 /*****************************************************************************
00072 ** Inflation
00073 *****************************************************************************/
00074 
00075 class Inflate {
00076 public:
00077   Inflate() : cell_inflation_radius_(std::numeric_limits<unsigned int>::max())
00078   {
00079   };
00080 
00091   void operator()(const std::string layer_source,
00092                const std::string layer_destination,
00093                const float& inflation_radius,
00094                const InflationComputer& inflation_computer,
00095                CostMap& cost_map
00096                );
00097 
00098 private:
00099   struct CellData {
00109     CellData(double d, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
00110         distance_(d), x_(x), y_(y), src_x_(sx), src_y_(sy)
00111     {
00112     }
00117     friend bool operator<(const CellData &a, const CellData &b) {
00118       return a.distance_ > b.distance_;
00119     }
00120     double distance_;
00121     unsigned int x_, y_;
00122     unsigned int src_x_, src_y_;
00123   };
00124 
00133   void enqueue(const cost_map::Matrix& data_source,
00134                cost_map::Matrix& data_destination,
00135                unsigned int mx, unsigned int my,
00136                unsigned int src_x, unsigned int src_y
00137                );
00138 
00147   double distanceLookup(int mx, int my, int src_x, int src_y);
00148 
00157   unsigned char costLookup(int mx, int my, int src_x, int src_y);
00158   void computeCaches(const float& resolution, const InflationComputer& compute_cost);
00159 
00160   Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> seen_;
00161   Eigen::MatrixXf cached_distances_;
00162   cost_map::Matrix cached_costs_;
00163   std::priority_queue<CellData> inflation_queue_;
00164   unsigned int cell_inflation_radius_; 
00165 };
00166 
00167 /*****************************************************************************
00168 ** Deflation
00169 *****************************************************************************/
00170 
00174 class Deflate {
00175 public:
00176   Deflate(const bool& do_not_strip_inscribed_region=false);
00177 
00186   void operator()(const std::string layer_source,
00187                   const std::string layer_destination,
00188                   CostMap& cost_map
00189                  );
00190 
00191 private:
00192   bool do_not_strip_inscribed_region;
00193 };
00194 
00195 /*****************************************************************************
00196 ** Trailers
00197 *****************************************************************************/
00198 
00199 } // namespace cost_map_core
00200 
00201 #endif /* cost_map_core_INFLATION_HPP_ */


cost_map_core
Author(s): Daniel Stonier
autogenerated on Thu Jun 6 2019 20:27:46