Add general optimizations and modern syntax

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

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_

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<unsigned int>(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<unsigned char>((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];
   }
 

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -37,7 +37,6 @@
  *********************************************************************/
 #include "nav2_costmap_2d/inflation_layer.hpp"
 
-#include <algorithm>
 #include <limits>
 #include <map>
 #include <vector>
@@ -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);
@@ -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);
       }
     }
   }
@@ -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
@@ -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);
   }
 }