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() {

1157
plugins/obstacle_layer.cpp
View File

File diff suppressed because it is too large Load Diff

251
plugins/static_layer.cpp
View File

@@ -1,48 +1,10 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* Copyright (c) 2015, Fetch Robotics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)
#include <tf2/utils.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
@@ -51,92 +13,78 @@ using costmap_2d::FREE_SPACE;
namespace costmap_2d
{
StaticLayer::StaticLayer() : dsrv_(NULL)
StaticLayer::StaticLayer()
{
threshold_ = &lethal_threshold_;
}
StaticLayer::~StaticLayer()
{
if (dsrv_)
delete dsrv_;
}
{}
void StaticLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_), g_nh;
// ros::NodeHandle nh("~/" + name_), g_nh;
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
nh.param("map_topic", map_topic_, std::string("map"));
nh.param("first_map_only", first_map_only_, false);
nh.param("subscribe_to_updates", subscribe_to_updates_, false);
// nh.param("map_topic", map_topic_, std::string("map"));
// nh.param("first_map_only", first_map_only_, false);
// nh.param("subscribe_to_updates", subscribe_to_updates_, false);
nh.param("track_unknown_space", track_unknown_space_, true);
nh.param("use_maximum", use_maximum_, false);
// nh.param("track_unknown_space", track_unknown_space_, true);
// nh.param("use_maximum", use_maximum_, false);
int temp_lethal_threshold, temp_unknown_cost_value;
nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
nh.param("trinary_costmap", trinary_costmap_, true);
nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
// int temp_lethal_threshold, temp_unknown_cost_value;
// nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
// nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
// nh.param("trinary_costmap", trinary_costmap_, true);
// nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
// ROS_WARN("global_frame_[%s] map_frame_[%s] map_topic[%s] first_map_only[%d] subscribe_to_updates[%d] track_unknown_space[%d] use_maximum[%d] \n lethal_cost_threshold[%d] unknown_cost_value[%d] trinary_costmap[%d]",
// global_frame_.c_str(), map_frame_.c_str(),map_topic_.c_str(), first_map_only_, subscribe_to_updates_, track_unknown_space_, use_maximum_, temp_lethal_threshold, temp_unknown_cost_value, trinary_costmap_);
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
unknown_cost_value_ = temp_unknown_cost_value;
// lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
// unknown_cost_value_ = temp_unknown_cost_value;
// Only resubscribe if topic has changed
if (map_sub_.getTopic() != ros::names::resolve(map_topic_))
{
// we'll subscribe to the latched topic that the map server uses
ROS_INFO("Requesting the map...");
map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this);
map_received_ = false;
has_updated_data_ = false;
// if (map_sub_.getTopic() != ros::names::resolve(map_topic_))
// {
// // we'll subscribe to the latched topic that the map server uses
// printf("Requesting the map...");
// map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this);
// map_received_ = false;
// has_updated_data_ = false;
ros::Rate r(10);
while (!map_received_ && g_nh.ok())
{
ros::spinOnce();
r.sleep();
}
// ros::Rate r(10);
// while (!map_received_ && g_nh.ok())
// {
// ros::spinOnce();
// r.sleep();
// }
ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
if (subscribe_to_updates_)
{
ROS_INFO("Subscribing to updates");
map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
}
}
else
{
has_updated_data_ = true;
}
// if (subscribe_to_updates_)
// {
// printf("Subscribing to updates");
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
// }
// }
// else
// {
// has_updated_data_ = true;
// }
if (dsrv_)
{
delete dsrv_;
}
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
[this](auto& config, auto level){ reconfigureCB(config, level); };
dsrv_->setCallback(cb);
}
void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
if (config.enabled != enabled_)
{
enabled_ = config.enabled;
has_updated_data_ = true;
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
}
}
// void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
// {
// if (config.enabled != enabled_)
// {
// enabled_ = config.enabled;
// has_updated_data_ = true;
// x_ = y_ = 0;
// width_ = size_x_;
// height_ = size_y_;
// }
// }
void StaticLayer::matchSize()
{
@@ -166,36 +114,36 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
return scale * LETHAL_OBSTACLE;
}
void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
printf("Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
(master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map->info.resolution ||
master->getOriginX() != new_map->info.origin.position.x ||
master->getOriginY() != new_map->info.origin.position.y))
master->getResolution() != new_map.info.resolution ||
master->getOriginX() != new_map.info.origin.position.x ||
master->getOriginY() != new_map.info.origin.position.y))
{
// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
new_map->info.origin.position.y,
printf("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
}
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map->info.resolution ||
origin_x_ != new_map->info.origin.position.x ||
origin_y_ != new_map->info.origin.position.y)
resolution_ != new_map.info.resolution ||
origin_x_ != new_map.info.origin.position.x ||
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
resizeMap(size_x, size_y, new_map->info.resolution,
new_map->info.origin.position.x, new_map->info.origin.position.y);
printf("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
resizeMap(size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
unsigned int index = 0;
@@ -205,12 +153,12 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map->data[index];
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
map_frame_ = new_map->header.frame_id;
map_frame_ = new_map.header.frame_id;
// we have a new map, update full size of map
x_ = y_ = 0;
@@ -222,27 +170,27 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
map_sub_.shutdown();
printf("Shutting down the map subscriber. first_map_only flag is on");
// map_sub_.shutdown();
}
}
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
{
unsigned int di = 0;
for (unsigned int y = 0; y < update->height ; y++)
for (unsigned int y = 0; y < update.height ; y++)
{
unsigned int index_base = (update->y + y) * size_x_;
for (unsigned int x = 0; x < update->width ; x++)
unsigned int index_base = (update.y + y) * size_x_;
for (unsigned int x = 0; x < update.width ; x++)
{
unsigned int index = index_base + x + update->x;
costmap_[index] = interpretValue(update->data[di++]);
unsigned int index = index_base + x + update.x;
costmap_[index] = interpretValue(update.data[di++]);
}
}
x_ = update->x;
y_ = update->y;
width_ = update->width;
height_ = update->height;
x_ = update.x;
y_ = update.y;
width_ = update.width;
height_ = update.height;
has_updated_data_ = true;
}
@@ -251,12 +199,12 @@ void StaticLayer::activate()
onInitialize();
}
void StaticLayer::deactivate()
{
map_sub_.shutdown();
if (subscribe_to_updates_)
map_update_sub_.shutdown();
}
// void StaticLayer::deactivate()
// {
// map_sub_.shutdown();
// if (subscribe_to_updates_)
// map_update_sub_.shutdown();
// }
void StaticLayer::reset()
{
@@ -314,18 +262,23 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
double wx, wy;
// Might even be in a different frame
geometry_msgs::TransformStamped transform;
tf2::TransformStampedMsg transformMsg;
try
{
transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
tf_->canTransform(map_frame_, global_frame_, tf2::Time());
tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
}
catch (tf2::TransformException ex)
{
ROS_ERROR("%s", ex.what());
printf("%s", ex.what());
return;
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
tf2::convert(transform.transform, tf2_transform);
// tf2::convert(transform.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
@@ -334,7 +287,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf2::Vector3 p(wx, wy, 0);
p = tf2_transform*p;
// p = tf2_transform*p;
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
@@ -348,4 +301,12 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
}
// Export factory function
static PluginPtr create_static_plugin() {
return std::make_shared<StaticLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_static_plugin, create_plugin)
} // namespace costmap_2d