From 06c35504e461d8c3233ce76ca35d63e61f95429a Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 3 Nov 2023 13:25:53 -0300 Subject: [PATCH] Handle NaNs in AMCL beam sensor model (#3929) * Handle NaNs in AMCL beam sensor model Signed-off-by: Michel Hidalgo * Use proper isnan check Signed-off-by: Michel Hidalgo --------- Signed-off-by: Michel Hidalgo --- nav2_amcl/src/sensors/laser/beam_model.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_amcl/src/sensors/laser/beam_model.cpp b/nav2_amcl/src/sensors/laser/beam_model.cpp index ebbfedcc7b..b6f281cbf7 100644 --- 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