void InflationLayer::onFootprintChanged()
{
inscribed_radius_ = layered_costmap_->getInscribedRadius();
cell_inflation_radius_ = cellDistance( inflation_radius_ );
computeCaches();
need_reinflation_ = true;
ROS_DEBUG( "InflationLayer::onFootprintChanged(): num footprint points: %lu, inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_ );
}
void InflationLayer::matchSize()
{
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
if (seen_)
delete seen_;
seen_ = new bool[size_x * size_y];
}
void InflationLayer::matchSize()
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
if (seen_)
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
{
if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
{
// Lock here so that reconfiguring the inflation radius doesn't cause segfaults
// when accessing the cached arrays
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
inflation_radius_ = inflation_radius;
cell_inflation_radius_ = cellDistance(inflation_radius_);
weight_ = cost_scaling_factor;
need_reinflation_ = true;
computeCaches();
}
}
void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
{
if (weight_ != config.cost_scaling_factor || inflation_radius_ != config.inflation_radius)
{
inflation_radius_ = config.inflation_radius;
cell_inflation_radius_ = cellDistance(inflation_radius_);
weight_ = config.cost_scaling_factor;
need_reinflation_ = true;
computeCaches();
}
if (enabled_ != config.enabled) {
enabled_ = config.enabled;
need_reinflation_ = true;
}
}
