Skip to content
Snippets Groups Projects
Unverified Commit 1b7c67f2 authored by Shrijit Singh's avatar Shrijit Singh Committed by GitHub
Browse files

Add option to inflate unknown space (#1675)


* Add option to inflate around unknown space

Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>

* Fix bug regarding lower bound of double in worldToMapEnforceBounds

Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>

* Convert 2D caches to 1D vectors for automatic memory management and better locality

Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>

* Add general optimizations and modern syntax

Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>

* Switch from map<double> to vector<> in using precached integer distances

Credits to original author from https://github.com/ros-planning/navigation/pull/839



Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>

* Add tests for inflate_unknown and inflate_around_unknown

Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>

* Remove commented out assert

Signed-off-by: default avatarShrijit Singh <shrijitsingh99@gmail.com>
parent 2e665e44
No related branches found
No related tags found
No related merge requests found
......@@ -39,9 +39,9 @@
/** Provides a mapping for often used cost values */
namespace nav2_costmap_2d
{
static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static const unsigned char FREE_SPACE = 0;
static constexpr unsigned char NO_INFORMATION = 255;
static constexpr unsigned char LETHAL_OBSTACLE = 254;
static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static constexpr unsigned char FREE_SPACE = 0;
}
#endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_
......@@ -64,7 +64,7 @@ public:
* @return
*/
CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
: index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
: index_(static_cast<unsigned int>(i)), x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
unsigned int index_;
......@@ -77,27 +77,21 @@ class InflationLayer : public Layer
public:
InflationLayer();
virtual ~InflationLayer()
{
deleteKernels();
}
~InflationLayer() override = default;
virtual void onInitialize();
virtual void updateBounds(
void onInitialize() override;
void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y,
double * max_x,
double * max_y);
virtual void updateCosts(
double * max_y) override;
void updateCosts(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j);
virtual bool isDiscretized()
{
return true;
}
virtual void matchSize();
int min_i, int min_j, int max_i, int max_j) override;
void matchSize() override;
virtual void reset()
void reset() override
{
matchSize();
}
......@@ -114,15 +108,15 @@ public:
cost = INSCRIBED_INFLATED_OBSTACLE;
} else {
// make sure cost falls off by Euclidean distance
double euclidean_distance = distance * resolution_;
double factor = exp(-1.0 * cost_scaling_factor_ * (euclidean_distance - inscribed_radius_));
cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
double factor =
exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_));
cost = static_cast<unsigned char>((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
}
protected:
virtual void onFootprintChanged();
void onFootprintChanged() override;
private:
/**
......@@ -133,11 +127,13 @@ private:
* @param src_y The y coordinate of the source cell
* @return
*/
inline double distanceLookup(int mx, int my, int src_x, int src_y)
inline double distanceLookup(
unsigned int mx, unsigned int my, unsigned int src_x,
unsigned int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_distances_[dx][dy];
unsigned int dx = (mx > src_x) ? mx - src_x : src_x - mx;
unsigned int dy = (my > src_y) ? my - src_y : src_y - my;
return cached_distances_[dx * cache_length_ + dy];
}
/**
......@@ -148,16 +144,18 @@ private:
* @param src_y The y coordinate of the source cell
* @return
*/
inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
inline unsigned char costLookup(
unsigned int mx, unsigned int my, unsigned int src_x,
unsigned int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_costs_[dx][dy];
unsigned int dx = (mx > src_x) ? mx - src_x : src_x - mx;
unsigned int dy = (my > src_y) ? my - src_y : src_y - my;
return cached_costs_[dx * cache_length_ + dy];
}
void computeCaches();
void deleteKernels();
void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char * master_grid);
int generateIntegerDistances();
unsigned int cellDistance(double world_dist)
{
......@@ -169,17 +167,19 @@ private:
unsigned int src_x, unsigned int src_y);
double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
bool inflate_unknown_;
bool inflate_unknown_, inflate_around_unknown_;
unsigned int cell_inflation_radius_;
unsigned int cached_cell_inflation_radius_;
std::map<double, std::vector<CellData>> inflation_cells_;
std::vector<std::vector<CellData>> inflation_cells_;
double resolution_;
std::vector<bool> seen_;
unsigned char ** cached_costs_;
double ** cached_distances_;
std::vector<unsigned char> cached_costs_;
std::vector<double> cached_distances_;
std::vector<std::vector<int>> distance_matrix_;
unsigned int cache_length_;
double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
// Indicates that the entire costmap should be reinflated next time around.
......
......@@ -98,8 +98,8 @@ void addObstacleLayer(
}
void addObservation(
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z)
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true)
{
sensor_msgs::msg::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
......@@ -119,7 +119,7 @@ void addObservation(
// obstacle range = raytrace range = 100.0
nav2_costmap_2d::Observation obs(p, cloud, 100.0, 100.0);
olayer->addStaticObservation(obs, true, true);
olayer->addStaticObservation(obs, marking, clearing);
}
void addInflationLayer(
......
......@@ -37,7 +37,6 @@
*********************************************************************/
#include "nav2_costmap_2d/inflation_layer.hpp"
#include <algorithm>
#include <limits>
#include <map>
#include <vector>
......@@ -61,14 +60,14 @@ InflationLayer::InflationLayer()
inscribed_radius_(0),
cost_scaling_factor_(0),
inflate_unknown_(false),
inflate_around_unknown_(false),
cell_inflation_radius_(0),
cached_cell_inflation_radius_(0),
cached_costs_(nullptr),
cached_distances_(nullptr),
last_min_x_(-std::numeric_limits<float>::max()),
last_min_y_(-std::numeric_limits<float>::max()),
last_max_x_(std::numeric_limits<float>::max()),
last_max_y_(std::numeric_limits<float>::max())
cache_length_(0),
last_min_x_(std::numeric_limits<double>::lowest()),
last_min_y_(std::numeric_limits<double>::lowest()),
last_max_x_(std::numeric_limits<double>::max()),
last_max_y_(std::numeric_limits<double>::max())
{
}
......@@ -79,14 +78,18 @@ InflationLayer::onInitialize()
declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
declareParameter("inflate_unknown", rclcpp::ParameterValue(false));
declareParameter("inflate_around_unknown", rclcpp::ParameterValue(false));
node_->get_parameter(name_ + "." + "enabled", enabled_);
node_->get_parameter(name_ + "." + "inflation_radius", inflation_radius_);
node_->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_);
node_->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_);
node_->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_);
current_ = true;
seen_.clear();
cached_distances_.clear();
cached_costs_.clear();
need_reinflation_ = false;
cell_inflation_radius_ = cellDistance(inflation_radius_);
matchSize();
......@@ -112,13 +115,11 @@ InflationLayer::updateBounds(
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();
*min_x = std::numeric_limits<double>::lowest();
*min_y = std::numeric_limits<double>::lowest();
*max_x = std::numeric_limits<double>::max();
*max_y = std::numeric_limits<double>::max();
need_reinflation_ = false;
} else {
double tmp_min_x = last_min_x_;
......@@ -162,9 +163,11 @@ InflationLayer::updateCosts(
}
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
RCLCPP_FATAL_EXPRESSION(
rclcpp::get_logger("nav2_costmap_2d"),
!inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
for (auto & dist:inflation_cells_) {
RCLCPP_FATAL_EXPRESSION(
rclcpp::get_logger("nav2_costmap_2d"),
!dist.empty(), "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();
......@@ -182,10 +185,10 @@ InflationLayer::updateCosts(
// 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 -= static_cast<int>(cell_inflation_radius_);
min_j -= static_cast<int>(cell_inflation_radius_);
max_i += static_cast<int>(cell_inflation_radius_);
max_j += static_cast<int>(cell_inflation_radius_);
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
......@@ -198,13 +201,13 @@ InflationLayer::updateCosts(
// with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData> & obs_bin = inflation_cells_[0.0];
auto & obs_bin = inflation_cells_[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);
int index = static_cast<int>(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));
if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) {
obs_bin.emplace_back(index, i, j, i, j);
}
}
}
......@@ -212,13 +215,11 @@ InflationLayer::updateCosts(
// 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 (unsigned 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_;
for (const auto & dist_bin: inflation_cells_) {
for (std::size_t i = 0; i < dist_bin.size(); ++i) {
// Do not use iterator or for-range based loops to iterate though dist_bin, since it's size might
// change when a new cell is enqueued, invalidating all iterators
unsigned int index = dist_bin[i].index_;
// ignore if already visited
if (seen_[index]) {
......@@ -227,10 +228,10 @@ InflationLayer::updateCosts(
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 = dist_bin[i].x_;
unsigned int my = dist_bin[i].y_;
unsigned int sx = dist_bin[i].src_x_;
unsigned int sy = dist_bin[i].src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
......@@ -259,7 +260,10 @@ InflationLayer::updateCosts(
}
}
inflation_cells_.clear();
for (auto & dist:inflation_cells_) {
dist.clear();
dist.reserve(200);
}
}
/**
......@@ -287,8 +291,11 @@ InflationLayer::enqueue(
return;
}
const unsigned int r = cell_inflation_radius_ + 2;
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(
index, mx, my, src_x, src_y);
}
}
......@@ -299,55 +306,74 @@ InflationLayer::computeCaches()
return;
}
cache_length_ = cell_inflation_radius_ + 2;
// based on the inflation radius... compute distance and cost caches
if (cell_inflation_radius_ != cached_cell_inflation_radius_) {
deleteKernels();
cached_costs_.resize(cache_length_ * cache_length_);
cached_distances_.resize(cache_length_ * cache_length_);
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 < cache_length_; ++i) {
for (unsigned int j = 0; j < cache_length_; ++j) {
cached_distances_[i * cache_length_ + j] = hypot(i, j);
}
}
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 < cache_length_; ++i) {
for (unsigned int j = 0; j < cache_length_; ++j) {
cached_costs_[i * cache_length_ + j] = computeCost(cached_distances_[i * cache_length_ + j]);
}
}
int max_dist = generateIntegerDistances();
inflation_cells_.clear();
inflation_cells_.resize(max_dist + 1);
for (auto & dist : inflation_cells_) {
dist.reserve(200);
}
}
void
InflationLayer::deleteKernels()
int
InflationLayer::generateIntegerDistances()
{
if (cached_distances_ != NULL) {
for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) {
if (cached_distances_[i]) {
delete[] cached_distances_[i];
const int r = cell_inflation_radius_ + 2;
const int size = r * 2 + 1;
std::vector<std::pair<int, int>> points;
for (int y = -r; y <= r; y++) {
for (int x = -r; x <= r; x++) {
if (x * x + y * y <= r * r) {
points.emplace_back(x, y);
}
}
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];
}
std::sort(
points.begin(), points.end(),
[](const std::pair<int, int> & a, const std::pair<int, int> & b) -> bool {
return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second;
}
delete[] cached_costs_;
cached_costs_ = NULL;
);
std::vector<std::vector<int>> distance_matrix(size, std::vector<int>(size, 0));
std::pair<int, int> last = {0, 0};
int level = 0;
for (auto const & p : points) {
if (p.first * p.first + p.second * p.second !=
last.first * last.first + last.second * last.second)
{
level++;
}
distance_matrix[p.first + r][p.second + r] = level;
last = p;
}
distance_matrix_ = distance_matrix;
return level;
}
} // namespace nav2_costmap_2d
......@@ -129,8 +129,8 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
return;
}
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
minx_ = miny_ = std::numeric_limits<double>::max();
maxx_ = maxy_ = std::numeric_limits<double>::lowest();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
plugin != plugins_.end(); ++plugin)
......
......@@ -75,6 +75,7 @@ public:
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius);
void initNode(std::vector<rclcpp::Parameter> parameters);
void initNode(double inflation_radius);
void waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer);
......@@ -169,13 +170,8 @@ void TestNode::validatePointInflation(
delete[] seen;
}
void TestNode::initNode(double inflation_radius)
void TestNode::initNode(std::vector<rclcpp::Parameter> parameters)
{
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius));
auto options = rclcpp::NodeOptions();
options.parameter_overrides(parameters);
......@@ -195,6 +191,16 @@ void TestNode::initNode(double inflation_radius)
node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
}
void TestNode::initNode(double inflation_radius)
{
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius));
initNode(parameters);
}
TEST_F(TestNode, testAdjacentToObstacleCanStillMove)
{
initNode(4.1);
......@@ -254,6 +260,73 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns)
EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0u);
}
TEST_F(TestNode, testInflationInUnkown)
{
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", 4.1));
parameters.push_back(rclcpp::Parameter("inflation.inflate_unknown", true));
initNode(parameters);
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
layers.resizeMap(9, 9, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
addObservation(olayer, 4, 4, MAX_Z, 0.0, 0.0, MAX_Z, true, false);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// Only the 4 corners of the map should remain unknown
EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 4u);
}
TEST_F(TestNode, testInflationAroundUnkown)
{
auto inflation_radius = 4.1;
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius));
parameters.push_back(rclcpp::Parameter("inflation.inflate_around_unknown", true));
initNode(parameters);
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
layers.updateMap(0, 0, 0);
layers.getCostmap()->setCost(4, 4, nav2_costmap_2d::NO_INFORMATION);
ilayer->updateCosts(*layers.getCostmap(), 0, 0, 8, 8);
validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
}
/**
* Test for the cost function correctness with a larger range and different values
*/
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment