#include <inflation.hpp>
Classes | |
struct | CellData |
Public Member Functions | |
Inflate () | |
void | operator() (const std::string layer_source, const std::string layer_destination, const float &inflation_radius, const InflationComputer &inflation_computer, CostMap &cost_map) |
Inflate... | |
Private Member Functions | |
void | computeCaches (const float &resolution, const InflationComputer &compute_cost) |
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. | |
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. | |
Private Attributes | |
cost_map::Matrix | cached_costs_ |
Eigen::MatrixXf | cached_distances_ |
unsigned int | cell_inflation_radius_ |
std::priority_queue< CellData > | inflation_queue_ |
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic > | seen_ |
Definition at line 75 of file inflation.hpp.
cost_map::Inflate::Inflate | ( | ) | [inline] |
Definition at line 77 of file inflation.hpp.
void cost_map::Inflate::computeCaches | ( | const float & | resolution, |
const InflationComputer & | compute_cost | ||
) | [private] |
Definition at line 131 of file inflation.cpp.
unsigned char cost_map::Inflate::costLookup | ( | int | mx, |
int | my, | ||
int | src_x, | ||
int | src_y | ||
) | [private] |
Lookup pre-computed costs.
mx | The x coordinate of the current cell |
my | The y coordinate of the current cell |
src_x | The x coordinate of the source cell |
src_y | The y coordinate of the source cell |
Definition at line 124 of file inflation.cpp.
double cost_map::Inflate::distanceLookup | ( | int | mx, |
int | my, | ||
int | src_x, | ||
int | src_y | ||
) | [private] |
Lookup pre-computed distances.
mx | The x coordinate of the current cell |
my | The y coordinate of the current cell |
src_x | The x coordinate of the source cell |
src_y | The y coordinate of the source cell |
Definition at line 117 of file inflation.cpp.
void cost_map::Inflate::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 | ||
) | [private] |
Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation.
data_destination | the costs (will be read and superimposed on this) |
mx | The x coordinate of the cell (can be computed from the index, but saves time to store it) |
my | The y coordinate of the cell (can be computed from the index, but saves time to store it) |
src_x | The x index of the obstacle point inflation started at |
src_y | The y index of the obstacle point inflation started at |
Definition at line 84 of file inflation.cpp.
void cost_map::Inflate::operator() | ( | const std::string | layer_source, |
const std::string | layer_destination, | ||
const float & | inflation_radius, | ||
const InflationComputer & | inflation_computer, | ||
CostMap & | cost_map | ||
) |
Inflate...
layer_source | |
layer_destination | |
inflation_radius | |
inscribed_radius | |
cost_map |
std::out_of_range | if no map layer with name `layer` is present. |
Definition at line 21 of file inflation.cpp.
Definition at line 162 of file inflation.hpp.
Eigen::MatrixXf cost_map::Inflate::cached_distances_ [private] |
Definition at line 161 of file inflation.hpp.
unsigned int cost_map::Inflate::cell_inflation_radius_ [private] |
Definition at line 164 of file inflation.hpp.
std::priority_queue<CellData> cost_map::Inflate::inflation_queue_ [private] |
Definition at line 163 of file inflation.hpp.
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> cost_map::Inflate::seen_ [private] |
Definition at line 160 of file inflation.hpp.