9 #include "../../../include/cost_map_core/operators/inflation.hpp" 22 const std::string layer_destination,
23 const float& inflation_radius,
29 unsigned int size_x = cost_map.
getSize().x();
30 unsigned int size_y = cost_map.
getSize().y();
31 unsigned int size = size_x*size_y;
33 cost_map.
add(layer_destination, data_source);
36 seen_.resize(size_x, size_y);
37 seen_.setConstant(
false);
38 unsigned int new_cell_inflation_radius =
static_cast<unsigned int>(std::max(0.0, std::ceil(inflation_radius / cost_map.
getResolution())));
46 for (
unsigned int j = 0, number_of_rows = data_source.rows(), number_of_columns = data_source.cols(); j < number_of_columns; ++j) {
47 for (
unsigned int i = 0; i < number_of_rows; ++i) {
48 unsigned char cost = data_source(i, j);
50 unsigned int index = j*number_of_rows + i;
51 enqueue(cost_map.
get(layer_source), cost_map.
get(layer_destination), i, j, i, j);
60 unsigned int mx = current_cell.
x_;
61 unsigned int my = current_cell.
y_;
62 unsigned int sx = current_cell.
src_x_;
63 unsigned int sy = current_cell.
src_y_;
70 enqueue(cost_map.
get(layer_source), cost_map.
get(layer_destination), mx - 1, my, sx, sy);
73 enqueue(cost_map.
get(layer_source), cost_map.
get(layer_destination), mx, my - 1, sx, sy);
75 if (mx < size_x - 1) {
76 enqueue(cost_map.
get(layer_source), cost_map.
get(layer_destination), mx + 1, my, sx, sy);
78 if (my < size_y - 1) {
79 enqueue(cost_map.
get(layer_source), cost_map.
get(layer_destination), mx, my + 1, sx, sy);
86 unsigned int mx,
unsigned int my,
87 unsigned int src_x,
unsigned int src_y
102 unsigned char cost =
costLookup(mx, my, src_x, src_y);
103 unsigned char old_cost = data_source(mx, my);
106 data_destination(mx, my) = cost;
108 data_destination(mx, my) = std::max(old_cost, cost);
111 seen_(mx, my) =
true;
112 CellData data(distance, mx, my, src_x, src_y);
119 unsigned int dx = abs(mx - src_x);
120 unsigned int dy = abs(my - src_y);
126 unsigned int dx = abs(mx - src_x);
127 unsigned int dy = abs(my - src_y);
154 const float& inscribed_radius,
156 : inscribed_radius_(inscribed_radius)
162 unsigned char cost = 0;
181 : do_not_strip_inscribed_region(do_not_strip_inscribed_region)
186 const std::string layer_destination,
194 cost_map.
add(layer_destination);
200 for (
unsigned int j = 0, number_of_rows = data_source.rows(), number_of_columns = data_source.cols(); j < number_of_columns; ++j) {
201 for (
unsigned int i = 0; i < number_of_rows; ++i) {
202 unsigned char cost = data_source(i, j);
unsigned char costLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed costs.
Function which can compute costs for the inflation layer.
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > Matrix
const unsigned char NO_INFORMATION
cost_map::Matrix cached_costs_
unsigned int cell_inflation_radius_
const Size & getSize() const
double getResolution() const
void add(const std::string &layer, const DataType value=NO_INFORMATION)
double distanceLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed distances.
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic > seen_
virtual unsigned char operator()(const float &distance) const
Given a distance, compute a cost.
const unsigned char LETHAL_OBSTACLE
Deflate(const bool &do_not_strip_inscribed_region=false)
ROSInflationComputer(const float &inscribed_radius, const float &weight)
const unsigned char INSCRIBED_OBSTACLE
const Matrix & get(const std::string &layer) const
void operator()(const std::string layer_source, const std::string layer_destination, CostMap &cost_map)
Deflate...
const unsigned char FREE_SPACE
void computeCaches(const float &resolution, const InflationComputer &compute_cost)
std::priority_queue< CellData > inflation_queue_
bool do_not_strip_inscribed_region
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)
Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation...
void operator()(const std::string layer_source, const std::string layer_destination, const float &inflation_radius, const InflationComputer &inflation_computer, CostMap &cost_map)
Inflate...
Eigen::MatrixXf cached_distances_