Add general optimizations

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>
ros-navigation · May 3, 2020 · 2f2803d · 2f2803d
1 parent 06fa325
commit 2f2803d
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 13 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
@@ -157,7 +157,6 @@ class InflationLayer : public Layer
 
   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)
   {

diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -207,7 +207,7 @@ InflationLayer::updateCosts(
       int index = 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);
       }
     }
   }
@@ -217,10 +217,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
@@ -291,7 +289,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(CellData(index, mx, my, src_x, src_y));
   }
 }
 
@@ -330,26 +328,25 @@ InflationLayer::computeCaches()
 void
 InflationLayer::deleteKernels()
 {
-  if (cached_distances_ != NULL) {
+  if (cached_distances_ != nullptr) {
     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;
+    delete[] cached_distances_;
+
+    cached_distances_ = nullptr;
   }
 
-  if (cached_costs_ != NULL) {
+  if (cached_costs_ != nullptr) {
     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;
+    cached_costs_ = nullptr;
   }
 }