[AMCL] Rebuild cspace map only when needed (#5076)

* for LikelihoodFieldModel

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* for LikelihoodFieldModelProb

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* typo

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* init max_occ_dist in map.c and remove unnecessary condition

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

Author: doisyg

nav2_amcl/src/map/map.c

@@ -49,6 +49,9 @@ map_t * map_alloc(void)
   // Allocate storage for main map
   map->cells = (map_cell_t *) NULL;
 
+  // Initialize max_occ_dist to 0.0
+  map->max_occ_dist = 0.0;
+
   return map;
 }
 

nav2_amcl/src/sensors/laser/likelihood_field_model.cpp

@@ -35,7 +35,15 @@ LikelihoodFieldModel::LikelihoodFieldModel(
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
-  map_update_cspace(map, max_occ_dist);
+
+  // recompute cspace only when necessary, i.e. if:
+  // - max_occ_dist changed
+  // OR
+  // - cspace was not computed yet, i.e. when map->max_occ_dist == 0.0 (and hence different from
+  // max_occ_dist)
+  if (map->max_occ_dist != max_occ_dist) {
+    map_update_cspace(map, max_occ_dist);
+  }
 }
 
 double

nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp

@@ -43,7 +43,15 @@ LikelihoodFieldModelProb::LikelihoodFieldModelProb(
   beam_skip_distance_ = beam_skip_distance;
   beam_skip_threshold_ = beam_skip_threshold;
   beam_skip_error_threshold_ = beam_skip_error_threshold;
-  map_update_cspace(map, max_occ_dist);
+
+  // recompute cspace only when necessary, i.e. if:
+  // - max_occ_dist changed
+  // OR
+  // - cspace was not computed yet, i.e. when map->max_occ_dist == 0.0 (and hence different from
+  // max_occ_dist)
+  if (map->max_occ_dist != max_occ_dist) {
+    map_update_cspace(map, max_occ_dist);
+  }
 }
 
 // Determine the probability for the given pose