Skip to content

Commit

Permalink
Add general optimizations and modern syntax
Browse files Browse the repository at this point in the history
Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>
  • Loading branch information
shrijitsingh99 committed May 4, 2020
1 parent ae8b0fa commit d21cf33
Show file tree
Hide file tree
Showing 3 changed files with 36 additions and 38 deletions.
8 changes: 4 additions & 4 deletions nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_
47 changes: 24 additions & 23 deletions nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<unsigned int>(i)), x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
unsigned int index_;
Expand All @@ -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();
}
Expand All @@ -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<unsigned char>((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
}

protected:
virtual void onFootprintChanged();
void onFootprintChanged() override;

private:
/**
Expand All @@ -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];
}

Expand All @@ -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];
}

Expand Down
19 changes: 8 additions & 11 deletions nav2_costmap_2d/plugins/inflation_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
*********************************************************************/
#include "nav2_costmap_2d/inflation_layer.hpp"

#include <algorithm>
#include <limits>
#include <map>
#include <vector>
Expand Down Expand Up @@ -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<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);
Expand All @@ -203,10 +202,10 @@ InflationLayer::updateCosts(
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);
int index = static_cast<int>(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);
}
}
}
Expand All @@ -216,10 +215,8 @@ InflationLayer::updateCosts(
// 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) {
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
Expand Down Expand Up @@ -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);
}
}

Expand Down

0 comments on commit d21cf33

Please sign in to comment.