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

* Handle NaNs in AMCL beam sensor model

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

* Use proper isnan check

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

---------

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
(cherry picked from commit 06c3550)

Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com>

Author: 2 people

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