Handle NaNs in AMCL beam sensor model (ros-navigation#3929)

* Handle NaNs in AMCL beam sensor model Signed-off-by: Michel Hidalgo <[email protected]> * Use proper isnan check Signed-off-by: Michel Hidalgo <[email protected]> --------- Signed-off-by: Michel Hidalgo <[email protected]>
kiwicampus · Nov 3, 2023 · 06c3550 · 06c3550
1 parent ef51e01
commit 06c3550
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/nav2_amcl/src/sensors/laser/beam_model.cpp b/nav2_amcl/src/sensors/laser/beam_model.cpp
@@ -72,6 +72,12 @@ BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
     step = (data->range_count - 1) / (self->max_beams_ - 1);
     for (i = 0; i < data->range_count; i += step) {
       obs_range = data->ranges[i][0];
+
+      // Check for NaN
+      if (isnan(obs_range)) {
+        continue;
+      }
+
       obs_bearing = data->ranges[i][1];
 
       // Compute the range according to the map