From 36e1a8af0a231db55347c7c60d4a507500d3f6dd Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Sun, 3 May 2020 18:49:43 +0530 Subject: [PATCH 1/7] Add option to inflate around unknown space Signed-off-by: Shrijit Singh --- nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index edc4803061..8fa1455583 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -169,7 +169,7 @@ class InflationLayer : public Layer 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> inflation_cells_; diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 574a155779..0262eea83b 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -61,6 +61,7 @@ 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), @@ -79,11 +80,13 @@ 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(); @@ -203,7 +206,7 @@ InflationLayer::updateCosts( 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) { + if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { obs_bin.push_back(CellData(index, i, j, i, j)); } } From 044aca907d9a917c813d68a7a13bfd3da2b09d5e Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Mon, 4 May 2020 14:00:14 +0530 Subject: [PATCH 2/7] Convert 2D caches to 1D vectors for automatic memory management and better locality Signed-off-by: Shrijit Singh --- .../nav2_costmap_2d/inflation_layer.hpp | 16 +++--- nav2_costmap_2d/plugins/inflation_layer.cpp | 53 +++++-------------- 2 files changed, 19 insertions(+), 50 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 8fa1455583..b98481e1e8 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -77,10 +77,7 @@ class InflationLayer : public Layer public: InflationLayer(); - virtual ~InflationLayer() - { - deleteKernels(); - } + virtual ~InflationLayer() = default; virtual void onInitialize(); virtual void updateBounds( @@ -137,7 +134,7 @@ class InflationLayer : public Layer { unsigned int dx = abs(mx - src_x); unsigned int dy = abs(my - src_y); - return cached_distances_[dx][dy]; + return cached_distances_[dx * cache_length_ + dy]; } /** @@ -152,12 +149,10 @@ class InflationLayer : public Layer { unsigned int dx = abs(mx - src_x); unsigned int dy = abs(my - src_y); - return cached_costs_[dx][dy]; + 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); unsigned int cellDistance(double world_dist) { @@ -178,8 +173,9 @@ class InflationLayer : public Layer std::vector seen_; - unsigned char ** cached_costs_; - double ** cached_distances_; + std::vector cached_costs_; + std::vector cached_distances_; + 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. diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 0262eea83b..44468a0e7e 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -64,8 +64,7 @@ InflationLayer::InflationLayer() inflate_around_unknown_(false), cell_inflation_radius_(0), cached_cell_inflation_radius_(0), - cached_costs_(nullptr), - cached_distances_(nullptr), + cache_length_(0), last_min_x_(-std::numeric_limits::max()), last_min_y_(-std::numeric_limits::max()), last_max_x_(std::numeric_limits::max()), @@ -90,6 +89,8 @@ InflationLayer::onInitialize() current_ = true; seen_.clear(); + cached_distances_.clear(); + cached_costs_.clear(); need_reinflation_ = false; cell_inflation_radius_ = cellDistance(inflation_radius_); matchSize(); @@ -302,54 +303,26 @@ 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_ = new unsigned char *[cell_inflation_radius_ + 2]; - cached_distances_ = new double *[cell_inflation_radius_ + 2]; + cached_costs_.resize(cache_length_ * cache_length_); + cached_distances_.resize(cache_length_ * cache_length_); - 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]); - } - } -} - -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]; - } + 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]); } - delete[] cached_costs_; - cached_costs_ = NULL; } } From eeb58e8d4b53ab5ce8f16d03f2937a71b257ebb9 Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Mon, 4 May 2020 12:56:40 +0530 Subject: [PATCH 3/7] Fix bug regarding lower bound of double in worldToMapEnforceBounds Signed-off-by: Shrijit Singh --- nav2_costmap_2d/plugins/inflation_layer.cpp | 20 +++++++++----------- nav2_costmap_2d/src/layered_costmap.cpp | 4 ++-- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 44468a0e7e..ea31d1f1e0 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -65,10 +65,10 @@ InflationLayer::InflationLayer() cell_inflation_radius_(0), cached_cell_inflation_radius_(0), cache_length_(0), - last_min_x_(-std::numeric_limits::max()), - last_min_y_(-std::numeric_limits::max()), - last_max_x_(std::numeric_limits::max()), - last_max_y_(std::numeric_limits::max()) + last_min_x_(std::numeric_limits::lowest()), + last_min_y_(std::numeric_limits::lowest()), + last_max_x_(std::numeric_limits::max()), + last_max_y_(std::numeric_limits::max()) { } @@ -116,13 +116,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 -::max() it does not - // work with Costmap2D::worldToMapEnforceBounds(), so I'm using - // -::max() instead. - *min_x = -std::numeric_limits::max(); - *min_y = -std::numeric_limits::max(); - *max_x = std::numeric_limits::max(); - *max_y = std::numeric_limits::max(); + + *min_x = std::numeric_limits::lowest(); + *min_y = std::numeric_limits::lowest(); + *max_x = std::numeric_limits::max(); + *max_y = std::numeric_limits::max(); need_reinflation_ = false; } else { double tmp_min_x = last_min_x_; diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 96cd1e6052..48ba1ea467 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -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::max(); + maxx_ = maxy_ = std::numeric_limits::lowest(); for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) From eee336a09afab92f1b3c09b347ad3adaf4ec324f Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Mon, 4 May 2020 14:02:44 +0530 Subject: [PATCH 4/7] Add general optimizations and modern syntax Signed-off-by: Shrijit Singh --- .../include/nav2_costmap_2d/cost_values.hpp | 8 ++-- .../nav2_costmap_2d/inflation_layer.hpp | 47 ++++++++++--------- nav2_costmap_2d/plugins/inflation_layer.cpp | 19 ++++---- 3 files changed, 36 insertions(+), 38 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 6452110c85..45725757f2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -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_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index b98481e1e8..62349b6ad4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -64,7 +64,7 @@ class CellData * @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(i)), x_(x), y_(y), src_x_(sx), src_y_(sy) { } unsigned int index_; @@ -77,24 +77,21 @@ class InflationLayer : public Layer public: InflationLayer(); - virtual ~InflationLayer() = default; + ~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(); } @@ -105,21 +102,21 @@ class InflationLayer : public Layer inline unsigned char computeCost(double distance) const { unsigned char cost = 0; + double euclidean_distance = distance * resolution_; if (distance == 0) { cost = LETHAL_OBSTACLE; - } else if (distance * resolution_ <= inscribed_radius_) { + } else if (euclidean_distance <= inscribed_radius_) { 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); + cost = static_cast((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } return cost; } protected: - virtual void onFootprintChanged(); + void onFootprintChanged() override; private: /** @@ -130,10 +127,12 @@ class InflationLayer : public Layer * @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); + 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]; } @@ -145,10 +144,12 @@ class InflationLayer : public Layer * @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); + 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]; } diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index ea31d1f1e0..96ca649689 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -37,7 +37,6 @@ *********************************************************************/ #include "nav2_costmap_2d/inflation_layer.hpp" -#include #include #include #include @@ -184,10 +183,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(cell_inflation_radius_); + min_j -= static_cast(cell_inflation_radius_); + max_i += static_cast(cell_inflation_radius_); + max_j += static_cast(cell_inflation_radius_); min_i = std::max(0, min_i); min_j = std::max(0, min_j); @@ -203,10 +202,10 @@ InflationLayer::updateCosts( std::vector & 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); + int index = static_cast(master_grid.getIndex(i, j)); unsigned char cost = master_array[index]; if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { - obs_bin.push_back(CellData(index, i, j, i, j)); + obs_bin.emplace_back(index, i, j, i, j); } } } @@ -216,10 +215,8 @@ InflationLayer::updateCosts( // can overtake previously inserted but farther away cells std::map>::iterator bin; for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) { - for (unsigned int i = 0; i < bin->second.size(); ++i) { + for (auto & cell : bin->second) { // process all cells at distance dist_bin.first - const CellData & cell = bin->second[i]; - unsigned int index = cell.index_; // ignore if already visited @@ -290,7 +287,7 @@ InflationLayer::enqueue( } // 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].emplace_back(index, mx, my, src_x, src_y); } } From f46fb01a706ef68f8b7844625b98374b10fc3d8e Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Mon, 4 May 2020 23:43:54 +0530 Subject: [PATCH 5/7] Switch from map to vector<> in using precached integer distances Credits to original author from https://github.com/ros-planning/navigation/pull/839 Signed-off-by: Shrijit Singh --- .../nav2_costmap_2d/inflation_layer.hpp | 11 ++- nav2_costmap_2d/plugins/inflation_layer.cpp | 85 +++++++++++++++---- 2 files changed, 77 insertions(+), 19 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 62349b6ad4..08fed09145 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -102,14 +102,14 @@ class InflationLayer : public Layer inline unsigned char computeCost(double distance) const { unsigned char cost = 0; - double euclidean_distance = distance * resolution_; if (distance == 0) { cost = LETHAL_OBSTACLE; - } else if (euclidean_distance <= inscribed_radius_) { + } else if (distance * resolution_ <= inscribed_radius_) { cost = INSCRIBED_INFLATED_OBSTACLE; } else { // make sure cost falls off by Euclidean distance - double factor = exp(-1.0 * cost_scaling_factor_ * (euclidean_distance - inscribed_radius_)); + double factor = + exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_)); cost = static_cast((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } return cost; @@ -155,6 +155,8 @@ class InflationLayer : public Layer void computeCaches(); + int generateIntegerDistances(); + unsigned int cellDistance(double world_dist) { return layered_costmap_->getCostmap()->cellDistance(world_dist); @@ -168,7 +170,7 @@ class InflationLayer : public Layer bool inflate_unknown_, inflate_around_unknown_; unsigned int cell_inflation_radius_; unsigned int cached_cell_inflation_radius_; - std::map> inflation_cells_; + std::vector> inflation_cells_; double resolution_; @@ -176,6 +178,7 @@ class InflationLayer : public Layer std::vector cached_costs_; std::vector cached_distances_; + std::vector> distance_matrix_; unsigned int cache_length_; double last_min_x_, last_min_y_, last_max_x_, last_max_y_; diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 96ca649689..86561d334b 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -163,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(); @@ -199,7 +201,7 @@ InflationLayer::updateCosts( // with a notable performance boost // Start with lethal obstacles: by definition distance is 0.0 - std::vector & 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 = static_cast(master_grid.getIndex(i, j)); @@ -213,11 +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>::iterator bin; - for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) { - for (auto & cell : bin->second) { - // process all cells at distance dist_bin.first - 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]) { @@ -226,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); @@ -258,7 +260,10 @@ InflationLayer::updateCosts( } } - inflation_cells_.clear(); + for (auto & dist:inflation_cells_) { + dist.clear(); + dist.reserve(200); + } } /** @@ -286,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].emplace_back(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); } } @@ -319,6 +327,53 @@ InflationLayer::computeCaches() 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); + } +} + +int +InflationLayer::generateIntegerDistances() +{ + const int r = cell_inflation_radius_ + 2; + const int size = r * 2 + 1; + + std::vector> 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); + } + } + } + + std::sort( + points.begin(), points.end(), + [](const std::pair & a, const std::pair & b) -> bool { + return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second; + } + ); + + std::vector> distance_matrix(size, std::vector(size, 0)); + std::pair 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 From bad22eb641eb84375b72c9ee9eefea5589733624 Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Thu, 14 May 2020 08:13:06 +0530 Subject: [PATCH 6/7] Add tests for inflate_unknown and inflate_around_unknown Signed-off-by: Shrijit Singh --- .../nav2_costmap_2d/testing_helper.hpp | 6 +- .../test/integration/inflation_tests.cpp | 88 +++++++++++++++++-- 2 files changed, 84 insertions(+), 10 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp index f8581c37bc..013341adf9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp @@ -98,8 +98,8 @@ void addObstacleLayer( } void addObservation( - std::shared_ptr & olayer, double x, double y, double z = 0.0, - double ox = 0.0, double oy = 0.0, double oz = MAX_Z) + std::shared_ptr 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( diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 0337590a17..bc03780c96 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -75,6 +75,7 @@ class TestNode : public ::testing::Test std::shared_ptr & ilayer, double inflation_radius); + void initNode(std::vector parameters); void initNode(double inflation_radius); void waitForMap(std::shared_ptr & slayer); @@ -137,7 +138,8 @@ void TestNode::validatePointInflation( double dist = std::hypot(dx, dy); unsigned char expected_cost = ilayer->computeCost(dist); - ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost); +// ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost) + costmap->setCost(cell.y_, cell.y_, expected_cost); if (dist > inflation_radius) { continue; @@ -169,13 +171,8 @@ void TestNode::validatePointInflation( delete[] seen; } -void TestNode::initNode(double inflation_radius) +void TestNode::initNode(std::vector parameters) { - std::vector 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 +192,16 @@ void TestNode::initNode(double inflation_radius) node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); } +void TestNode::initNode(double inflation_radius) +{ + std::vector 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 +261,73 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns) EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0u); } +TEST_F(TestNode, testInflationInUnkown) +{ + std::vector 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 polygon = setRadii(layers, 2.1, 2.3); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + std::shared_ptr 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 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 polygon = setRadii(layers, 2.1, 2.3); + + std::shared_ptr 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 */ From 5f2ddaed301cbdb581b8c672b0736df77d3687ce Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Thu, 14 May 2020 23:47:26 +0530 Subject: [PATCH 7/7] Remove commented out assert Signed-off-by: Shrijit Singh --- nav2_costmap_2d/test/integration/inflation_tests.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index bc03780c96..40262ce983 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -138,8 +138,7 @@ void TestNode::validatePointInflation( double dist = std::hypot(dx, dy); unsigned char expected_cost = ilayer->computeCost(dist); -// ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost) - costmap->setCost(cell.y_, cell.y_, expected_cost); + ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost); if (dist > inflation_radius) { continue;