DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1738_29102025

Browse Source
This commit is contained in:
duongtd
2025-10-29 17:38:43 +07:00
parent 7c1dcfd352
commit cdb9ded893
51 changed files with 2055 additions and 1216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

540
plugins/inflation_layer.cpp
View File

@@ -33,34 +33,48 @@ InflationLayer::InflationLayer()
inflation_access_ = new boost::recursive_mutex();
}
// void InflationLayer::onInitialize()
// {
// {
// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
// ros::NodeHandle nh("~/" + name_), g_nh;
// current_ = true;
// if (seen_)
// delete[] seen_;
// seen_ = NULL;
// seen_size_ = 0;
// need_reinflation_ = false;
void InflationLayer::onInitialize()
{
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
current_ = true;
if (seen_)
delete[] seen_;
seen_ = NULL;
seen_size_ = 0;
need_reinflation_ = false;
// dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb =
// [this](auto& config, auto level){ reconfigureCB(config, level); };
getParams();
}
// if (dsrv_ != NULL){
// dsrv_->clearCallback();
// dsrv_->setCallback(cb);
// }
// else
// {
// dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(ros::NodeHandle("~/" + name_));
// dsrv_->setCallback(cb);
// }
// }
matchSize();
}
// matchSize();
// }
bool InflationLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("../cfg/config.yaml");
double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as<double>();
double inflation_radius = config["inflation_layer"]["inflation_radius"].as<double>();
setInflationParameters(inflation_radius, cost_scaling_factor);
bool enabled = config["inflation_layer"]["enabled"].as<bool>();
bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as<bool>();
if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) {
enabled_ = enabled;
inflate_unknown_ = inflate_unknown;
need_reinflation_ = true;
}
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
// void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
// {
@@ -73,276 +87,278 @@ InflationLayer::InflationLayer()
// }
// }
// 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();
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_];
// }
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::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
// double* min_y, double* max_x, double* max_y)
// {
// if (need_reinflation_)
// {
// last_min_x_ = *min_x;
// last_min_y_ = *min_y;
// last_max_x_ = *max_x;
// last_max_y_ = *max_y;
// // For some reason when I make these -<double>::max() it does not
// // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
// // -<float>::max() instead.
// *min_x = -std::numeric_limits<float>::max();
// *min_y = -std::numeric_limits<float>::max();
// *max_x = std::numeric_limits<float>::max();
// *max_y = std::numeric_limits<float>::max();
// need_reinflation_ = false;
// }
// else
// {
// double tmp_min_x = last_min_x_;
// double tmp_min_y = last_min_y_;
// double tmp_max_x = last_max_x_;
// double tmp_max_y = last_max_y_;
// last_min_x_ = *min_x;
// last_min_y_ = *min_y;
// last_max_x_ = *max_x;
// last_max_y_ = *max_y;
// *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
// *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
// *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
// *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
// }
// }
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (need_reinflation_)
{
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
// For some reason when I make these -<double>::max() it does not
// work with Costmap2D::worldToMapEnforceBounds(), so I'm using
// -<float>::max() instead.
*min_x = -std::numeric_limits<float>::max();
*min_y = -std::numeric_limits<float>::max();
*max_x = std::numeric_limits<float>::max();
*max_y = std::numeric_limits<float>::max();
need_reinflation_ = false;
}
else
{
double tmp_min_x = last_min_x_;
double tmp_min_y = last_min_y_;
double tmp_max_x = last_max_x_;
double tmp_max_y = last_max_y_;
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
*min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
*min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
*max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
*max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
}
}
// void InflationLayer::onFootprintChanged()
// {
// inscribed_radius_ = layered_costmap_->getInscribedRadius();
// cell_inflation_radius_ = cellDistance(inflation_radius_);
// computeCaches();
// need_reinflation_ = true;
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_);
// }
printf("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
" inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}
// void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
// {
// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
// if (cell_inflation_radius_ == 0)
// return;
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (cell_inflation_radius_ == 0)
return;
// // make sure the inflation list is empty at the beginning of the cycle (should always be true)
// ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
if (!inflation_cells_.empty()) {
throw std::runtime_error("The inflation list must be empty at the beginning of inflation");
}
// unsigned char* master_array = master_grid.getCharMap();
// unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
// if (seen_ == NULL) {
// ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
// seen_size_ = size_x * size_y;
// seen_ = new bool[seen_size_];
// }
// else if (seen_size_ != size_x * size_y)
// {
// ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
// delete[] seen_;
// seen_size_ = size_x * size_y;
// seen_ = new bool[seen_size_];
// }
// memset(seen_, false, size_x * size_y * sizeof(bool));
if (seen_ == NULL) {
std::cerr <<"InflationLayer::updateCosts(): seen_ array is NULL" <<std::endl;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
std::cerr <<"InflationLayer::updateCosts(): seen_ array size is wrong" <<std::endl;
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// // We need to include in the inflation cells outside the bounding
// // box min_i...max_j, by the amount cell_inflation_radius_. Cells
// // up to that distance outside the box can still influence the costs
// // stored in cells inside the box.
// min_i -= cell_inflation_radius_;
// min_j -= cell_inflation_radius_;
// max_i += cell_inflation_radius_;
// max_j += cell_inflation_radius_;
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
// min_i = std::max(0, min_i);
// min_j = std::max(0, min_j);
// max_i = std::min(int(size_x), max_i);
// max_j = std::min(int(size_y), max_j);
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);
// // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// // Start with lethal obstacles: by definition distance is 0.0
// std::vector<CellData>& obs_bin = inflation_cells_[0.0];
// for (int j = min_j; j < max_j; j++)
// {
// for (int i = min_i; i < max_i; i++)
// {
// int index = master_grid.getIndex(i, j);
// unsigned char cost = master_array[index];
// if (cost == LETHAL_OBSTACLE)
// {
// obs_bin.push_back(CellData(index, i, j, i, j));
// }
// }
// }
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}
// // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// // can overtake previously inserted but farther away cells
// std::map<double, std::vector<CellData> >::iterator bin;
// for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
// {
// for (int i = 0; i < bin->second.size(); ++i)
// {
// // process all cells at distance dist_bin.first
// const CellData& cell = bin->second[i];
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
// unsigned int index = cell.index_;
unsigned int index = cell.index_;
// // ignore if already visited
// if (seen_[index])
// {
// continue;
// }
// ignore if already visited
if (seen_[index])
{
continue;
}
// seen_[index] = true;
seen_[index] = true;
// unsigned int mx = cell.x_;
// unsigned int my = cell.y_;
// unsigned int sx = cell.src_x_;
// unsigned int sy = cell.src_y_;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// // assign the cost associated with the distance from an obstacle to the cell
// unsigned char cost = costLookup(mx, my, sx, sy);
// unsigned char old_cost = master_array[index];
// if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
// master_array[index] = cost;
// else
// master_array[index] = std::max(old_cost, cost);
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);
// // attempt to put the neighbors of the current cell onto the inflation list
// if (mx > 0)
// enqueue(index - 1, mx - 1, my, sx, sy);
// if (my > 0)
// enqueue(index - size_x, mx, my - 1, sx, sy);
// if (mx < size_x - 1)
// enqueue(index + 1, mx + 1, my, sx, sy);
// if (my < size_y - 1)
// enqueue(index + size_x, mx, my + 1, sx, sy);
// }
// }
// attempt to put the neighbors of the current cell onto the inflation list
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
// inflation_cells_.clear();
// }
inflation_cells_.clear();
}
// /**
// * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
// * @param grid The costmap
// * @param index The index of the cell
// * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
// * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it)
// * @param src_x The x index of the obstacle point inflation started at
// * @param src_y The y index of the obstacle point inflation started at
// */
// inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
// unsigned int src_x, unsigned int src_y)
// {
// if (!seen_[index])
// {
// // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
// double distance = distanceLookup(mx, my, src_x, src_y);
/**
* @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
* @param grid The costmap
* @param index The index of the cell
* @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
* @param my The y coordinate of the cell (can be computed from the index, but saves time to store it)
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
*/
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
if (!seen_[index])
{
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
double distance = distanceLookup(mx, my, src_x, src_y);
// // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
// if (distance > cell_inflation_radius_)
// return;
// we only want to put the cell in the list if it is within the inflation radius of the obstacle point
if (distance > cell_inflation_radius_)
return;
// // push the cell data onto the inflation list and mark
// inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
// }
// }
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
}
}
// void InflationLayer::computeCaches()
// {
// if (cell_inflation_radius_ == 0)
// return;
void InflationLayer::computeCaches()
{
if (cell_inflation_radius_ == 0)
return;
// // based on the inflation radius... compute distance and cost caches
// if (cell_inflation_radius_ != cached_cell_inflation_radius_)
// {
// deleteKernels();
// based on the inflation radius... compute distance and cost caches
if (cell_inflation_radius_ != cached_cell_inflation_radius_)
{
deleteKernels();
// cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
// cached_distances_ = new double*[cell_inflation_radius_ + 2];
cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
cached_distances_ = new double*[cell_inflation_radius_ + 2];
// for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
// {
// cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
// cached_distances_[i] = new double[cell_inflation_radius_ + 2];
// for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
// {
// cached_distances_[i][j] = hypot(i, j);
// }
// }
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
cached_distances_[i] = new double[cell_inflation_radius_ + 2];
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{
cached_distances_[i][j] = hypot(i, j);
}
}
// cached_cell_inflation_radius_ = cell_inflation_radius_;
// }
cached_cell_inflation_radius_ = cell_inflation_radius_;
}
// for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
// {
// for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
// {
// cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
// }
// }
// }
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
}
// void InflationLayer::deleteKernels()
// {
// if (cached_distances_ != NULL)
// {
// for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
// {
// if (cached_distances_[i])
// delete[] cached_distances_[i];
// }
// if (cached_distances_)
// delete[] cached_distances_;
// cached_distances_ = NULL;
// }
void InflationLayer::deleteKernels()
{
if (cached_distances_ != NULL)
{
for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
{
if (cached_distances_[i])
delete[] cached_distances_[i];
}
if (cached_distances_)
delete[] cached_distances_;
cached_distances_ = NULL;
}
// if (cached_costs_ != NULL)
// {
// for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
// {
// if (cached_costs_[i])
// delete[] cached_costs_[i];
// }
// delete[] cached_costs_;
// cached_costs_ = NULL;
// }
// }
if (cached_costs_ != NULL)
{
for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
{
if (cached_costs_[i])
delete[] cached_costs_[i];
}
delete[] cached_costs_;
cached_costs_ = NULL;
}
}
// 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_);
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();
// }
// }
inflation_radius_ = inflation_radius;
cell_inflation_radius_ = cellDistance(inflation_radius_);
weight_ = cost_scaling_factor;
need_reinflation_ = true;
computeCaches();
}
}
// Export factory function
static PluginPtr create_inflation_plugin() {