[AMCL] 50% initialisation (and reset) speed improvement (ros-navigation#4941)

* index lookup and comparator optimisation

Signed-off-by: Guillaume Doisy <[email protected]>

* lint

Signed-off-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

Author: 2 people

nav2_amcl/src/map/map_cspace.cpp

@@ -26,15 +26,15 @@
 #include "nav2_amcl/map/map.hpp"
 
 /*
- * @class CellData
+ * @struct CellData
  * @brief Data about map cells
  */
-class CellData
+struct CellData
 {
-public:
   map_t * map_;
   unsigned int i_, j_;
   unsigned int src_i_, src_j_;
+  float occ_dist;
 };
 
 /*
@@ -83,9 +83,7 @@ class CachedDistanceMap
  */
 bool operator<(const CellData & a, const CellData & b)
 {
-  return a.map_->cells[MAP_INDEX(
-             a.map_, a.i_,
-             a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
+  return a.occ_dist > b.occ_dist;
 }
 
 /*
@@ -119,7 +117,9 @@ void enqueue(
   CachedDistanceMap * cdm,
   unsigned char * marked)
 {
-  if (marked[MAP_INDEX(map, i, j)]) {
+  const int map_index = MAP_INDEX(map, i, j);
+
+  if (marked[map_index]) {
     return;
   }
 
@@ -131,18 +131,13 @@ void enqueue(
     return;
   }
 
-  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
-
-  CellData cell;
-  cell.map_ = map;
-  cell.i_ = i;
-  cell.j_ = j;
-  cell.src_i_ = src_i;
-  cell.src_j_ = src_j;
+  map->cells[map_index].occ_dist = distance * map->scale;
 
-  Q.push(cell);
+  Q.emplace(CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
+      static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
+      map->cells[map_index].occ_dist});
 
-  marked[MAP_INDEX(map, i, j)] = 1;
+  marked[map_index] = 1;
 }
 
 /*
@@ -165,16 +160,18 @@ void map_update_cspace(map_t * map, double max_occ_dist)
   // Enqueue all the obstacle cells
   CellData cell;
   cell.map_ = map;
+  int loop_map_index;
   for (int i = 0; i < map->size_x; i++) {
     cell.src_i_ = cell.i_ = i;
     for (int j = 0; j < map->size_y; j++) {
-      if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
-        map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
+      loop_map_index = MAP_INDEX(map, i, j);
+      if (map->cells[loop_map_index].occ_state == +1) {
+        map->cells[loop_map_index].occ_dist = 0.0;
         cell.src_j_ = cell.j_ = j;
-        marked[MAP_INDEX(map, i, j)] = 1;
+        marked[loop_map_index] = 1;
         Q.push(cell);
       } else {
-        map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
+        map->cells[loop_map_index].occ_dist = max_occ_dist;
       }
     }
   }
@@ -183,27 +180,27 @@ void map_update_cspace(map_t * map, double max_occ_dist)
     CellData current_cell = Q.top();
     if (current_cell.i_ > 0) {
       enqueue(
-        map, current_cell.i_ - 1, current_cell.j_,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_ - 1, current_cell.j_,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (current_cell.j_ > 0) {
       enqueue(
-        map, current_cell.i_, current_cell.j_ - 1,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_, current_cell.j_ - 1,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
       enqueue(
-        map, current_cell.i_ + 1, current_cell.j_,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_ + 1, current_cell.j_,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
       enqueue(
-        map, current_cell.i_, current_cell.j_ + 1,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_, current_cell.j_ + 1,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
 
     Q.pop();