#include <inflation_layer.h>
Public Member Functions | |
unsigned char | computeCost (double distance) const |
Given a distance, compute a cost. | |
InflationLayer () | |
virtual bool | isDiscretized () |
virtual void | matchSize () |
Implement this to make this layer match the size of the parent costmap. | |
virtual void | onInitialize () |
This is called at the end of initialize(). Override to implement subclass-specific initialization. | |
virtual void | reset () |
void | setInflationParameters (double inflation_radius, double cost_scaling_factor) |
Change the values of the inflation radius parameters. | |
virtual void | updateBounds (double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y) |
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds. | |
virtual void | updateCosts (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) |
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds(). | |
virtual | ~InflationLayer () |
Protected Member Functions | |
virtual void | onFootprintChanged () |
LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint. | |
Protected Attributes | |
boost::recursive_mutex * | inflation_access_ |
Private Member Functions | |
unsigned int | cellDistance (double world_dist) |
void | computeCaches () |
unsigned char | costLookup (int mx, int my, int src_x, int src_y) |
Lookup pre-computed costs. | |
void | deleteKernels () |
double | distanceLookup (int mx, int my, int src_x, int src_y) |
Lookup pre-computed distances. | |
void | enqueue (unsigned int index, 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 list pending for obstacle inflation. | |
void | inflate_area (int min_i, int min_j, int max_i, int max_j, unsigned char *master_grid) |
void | reconfigureCB (costmap_2d::InflationPluginConfig &config, uint32_t level) |
Private Attributes | |
unsigned int | cached_cell_inflation_radius_ |
unsigned char ** | cached_costs_ |
double ** | cached_distances_ |
unsigned int | cell_inflation_radius_ |
dynamic_reconfigure::Server < costmap_2d::InflationPluginConfig > * | dsrv_ |
std::map< double, std::vector < CellData > > | inflation_cells_ |
double | inflation_radius_ |
double | inscribed_radius_ |
double | last_max_x_ |
double | last_max_y_ |
double | last_min_x_ |
double | last_min_y_ |
bool | need_reinflation_ |
Indicates that the entire costmap should be reinflated next time around. | |
double | resolution_ |
bool * | seen_ |
int | seen_size_ |
double | weight_ |
Definition at line 75 of file inflation_layer.h.
Definition at line 54 of file inflation_layer.cpp.
virtual costmap_2d::InflationLayer::~InflationLayer | ( | ) | [inline, virtual] |
Definition at line 80 of file inflation_layer.h.
unsigned int costmap_2d::InflationLayer::cellDistance | ( | double | world_dist | ) | [inline, private] |
Definition at line 165 of file inflation_layer.h.
void costmap_2d::InflationLayer::computeCaches | ( | ) | [private] |
Definition at line 304 of file inflation_layer.cpp.
unsigned char costmap_2d::InflationLayer::computeCost | ( | double | distance | ) | const [inline] |
Given a distance, compute a cost.
distance | The distance from an obstacle in cells |
Definition at line 102 of file inflation_layer.h.
unsigned char costmap_2d::InflationLayer::costLookup | ( | int | mx, |
int | my, | ||
int | src_x, | ||
int | src_y | ||
) | [inline, 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 154 of file inflation_layer.h.
void costmap_2d::InflationLayer::deleteKernels | ( | ) | [private] |
Definition at line 339 of file inflation_layer.cpp.
double costmap_2d::InflationLayer::distanceLookup | ( | int | mx, |
int | my, | ||
int | src_x, | ||
int | src_y | ||
) | [inline, 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 139 of file inflation_layer.h.
void costmap_2d::InflationLayer::enqueue | ( | unsigned int | index, |
unsigned int | mx, | ||
unsigned int | my, | ||
unsigned int | src_x, | ||
unsigned int | src_y | ||
) | [inline, private] |
Given an index of a cell in the costmap, place it into a list pending for obstacle inflation.
grid | The costmap |
index | The index of the cell |
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 287 of file inflation_layer.cpp.
void costmap_2d::InflationLayer::inflate_area | ( | int | min_i, |
int | min_j, | ||
int | max_i, | ||
int | max_j, | ||
unsigned char * | master_grid | ||
) | [private] |
virtual bool costmap_2d::InflationLayer::isDiscretized | ( | ) | [inline, virtual] |
Definition at line 91 of file inflation_layer.h.
void costmap_2d::InflationLayer::matchSize | ( | ) | [virtual] |
Implement this to make this layer match the size of the parent costmap.
Reimplemented from costmap_2d::Layer.
Definition at line 110 of file inflation_layer.cpp.
void costmap_2d::InflationLayer::onFootprintChanged | ( | ) | [protected, virtual] |
LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint.
Reimplemented from costmap_2d::Layer.
Definition at line 160 of file inflation_layer.cpp.
void costmap_2d::InflationLayer::onInitialize | ( | ) | [virtual] |
This is called at the end of initialize(). Override to implement subclass-specific initialization.
tf_, name_, and layered_costmap_ will all be set already when this is called.
Reimplemented from costmap_2d::Layer.
Definition at line 71 of file inflation_layer.cpp.
void costmap_2d::InflationLayer::reconfigureCB | ( | costmap_2d::InflationPluginConfig & | config, |
uint32_t | level | ||
) | [private] |
Definition at line 100 of file inflation_layer.cpp.
virtual void costmap_2d::InflationLayer::reset | ( | ) | [inline, virtual] |
Reimplemented from costmap_2d::Layer.
Definition at line 97 of file inflation_layer.h.
void costmap_2d::InflationLayer::setInflationParameters | ( | double | inflation_radius, |
double | cost_scaling_factor | ||
) |
Change the values of the inflation radius parameters.
inflation_radius | The new inflation radius |
cost_scaling_factor | The new weight |
Definition at line 365 of file inflation_layer.cpp.
void costmap_2d::InflationLayer::updateBounds | ( | double | robot_x, |
double | robot_y, | ||
double | robot_yaw, | ||
double * | min_x, | ||
double * | min_y, | ||
double * | max_x, | ||
double * | max_y | ||
) | [virtual] |
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds.
For more details, see "Layered Costmaps for Context-Sensitive Navigation", by Lu et. Al, IROS 2014.
Reimplemented from costmap_2d::Layer.
Definition at line 125 of file inflation_layer.cpp.
void costmap_2d::InflationLayer::updateCosts | ( | costmap_2d::Costmap2D & | master_grid, |
int | min_i, | ||
int | min_j, | ||
int | max_i, | ||
int | max_j | ||
) | [virtual] |
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
Reimplemented from costmap_2d::Layer.
Definition at line 172 of file inflation_layer.cpp.
unsigned int costmap_2d::InflationLayer::cached_cell_inflation_radius_ [private] |
Definition at line 175 of file inflation_layer.h.
unsigned char** costmap_2d::InflationLayer::cached_costs_ [private] |
Definition at line 183 of file inflation_layer.h.
double** costmap_2d::InflationLayer::cached_distances_ [private] |
Definition at line 184 of file inflation_layer.h.
unsigned int costmap_2d::InflationLayer::cell_inflation_radius_ [private] |
Definition at line 174 of file inflation_layer.h.
dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>* costmap_2d::InflationLayer::dsrv_ [private] |
Definition at line 187 of file inflation_layer.h.
boost::recursive_mutex* costmap_2d::InflationLayer::inflation_access_ [protected] |
Definition at line 128 of file inflation_layer.h.
std::map<double, std::vector<CellData> > costmap_2d::InflationLayer::inflation_cells_ [private] |
Definition at line 176 of file inflation_layer.h.
double costmap_2d::InflationLayer::inflation_radius_ [private] |
Definition at line 173 of file inflation_layer.h.
double costmap_2d::InflationLayer::inscribed_radius_ [private] |
Definition at line 173 of file inflation_layer.h.
double costmap_2d::InflationLayer::last_max_x_ [private] |
Definition at line 185 of file inflation_layer.h.
double costmap_2d::InflationLayer::last_max_y_ [private] |
Definition at line 185 of file inflation_layer.h.
double costmap_2d::InflationLayer::last_min_x_ [private] |
Definition at line 185 of file inflation_layer.h.
double costmap_2d::InflationLayer::last_min_y_ [private] |
Definition at line 185 of file inflation_layer.h.
bool costmap_2d::InflationLayer::need_reinflation_ [private] |
Indicates that the entire costmap should be reinflated next time around.
Definition at line 190 of file inflation_layer.h.
double costmap_2d::InflationLayer::resolution_ [private] |
Definition at line 178 of file inflation_layer.h.
bool* costmap_2d::InflationLayer::seen_ [private] |
Definition at line 180 of file inflation_layer.h.
int costmap_2d::InflationLayer::seen_size_ [private] |
Definition at line 181 of file inflation_layer.h.
double costmap_2d::InflationLayer::weight_ [private] |
Definition at line 173 of file inflation_layer.h.