inflation.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef cost_map_core_INFLATION_HPP_
#define cost_map_core_INFLATION_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include "../cost_map.hpp"
#include <limits>
#include <queue>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace cost_map {

/*****************************************************************************
** Inflation Function
*****************************************************************************/

class InflationComputer {
public:
  InflationComputer() {};
  virtual ~InflationComputer() {};

  virtual unsigned char operator()(const float &distance) const = 0;
};

class ROSInflationComputer : public InflationComputer {
public:
  ROSInflationComputer(const float& inscribed_radius, const float& weight);

  virtual ~ROSInflationComputer() {};

  virtual unsigned char operator()(const float &distance) const;
private:
  float inscribed_radius_, weight_;
};

/*****************************************************************************
** Inflation
*****************************************************************************/

class Inflate {
public:
  Inflate() : cell_inflation_radius_(std::numeric_limits<unsigned int>::max())
  {
  };

  void operator()(const std::string layer_source,
               const std::string layer_destination,
               const float& inflation_radius,
               const InflationComputer& inflation_computer,
               CostMap& cost_map
               );

private:
  struct CellData {
    CellData(double d, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
        distance_(d), x_(x), y_(y), src_x_(sx), src_y_(sy)
    {
    }
    friend bool operator<(const CellData &a, const CellData &b) {
      return a.distance_ > b.distance_;
    }
    double distance_;
    unsigned int x_, y_;
    unsigned int src_x_, src_y_;
  };

  void enqueue(const cost_map::Matrix& data_source,
               cost_map::Matrix& data_destination,
               unsigned int mx, unsigned int my,
               unsigned int src_x, unsigned int src_y
               );

  double distanceLookup(int mx, int my, int src_x, int src_y);

  unsigned char costLookup(int mx, int my, int src_x, int src_y);
  void computeCaches(const float& resolution, const InflationComputer& compute_cost);

  Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> seen_;
  Eigen::MatrixXf cached_distances_;
  cost_map::Matrix cached_costs_;
  std::priority_queue<CellData> inflation_queue_;
  unsigned int cell_inflation_radius_; 
};

/*****************************************************************************
** Deflation
*****************************************************************************/

class Deflate {
public:
  Deflate(const bool& do_not_strip_inscribed_region=false);

  void operator()(const std::string layer_source,
                  const std::string layer_destination,
                  CostMap& cost_map
                 );

private:
  bool do_not_strip_inscribed_region;
};

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace cost_map_core

#endif /* cost_map_core_INFLATION_HPP_ */