From 695125ba0b52c5b98737aec7598b23fcb75cd255 Mon Sep 17 00:00:00 2001 From: Chuanhong Guo Date: Tue, 4 Apr 2023 14:54:39 +0800 Subject: [PATCH] Push NAN into point array for measurement outside range limit According to the comment in sensor_msgs/LaserScan, values < range_min or > range_max should be discarded. Without this, scan matching will fail for lidars publishing 0 for invalid measurements. --- include/slam_toolbox/laser_utils.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/slam_toolbox/laser_utils.hpp b/include/slam_toolbox/laser_utils.hpp index 56c2e1041..ceb4836d5 100644 --- a/include/slam_toolbox/laser_utils.hpp +++ b/include/slam_toolbox/laser_utils.hpp @@ -37,18 +37,20 @@ inline std::vector scanToReadings( const bool & inverted) { std::vector readings; + float range_min = scan.range_min; + float range_max = scan.range_max; if (inverted) { for (std::vector::const_reverse_iterator it = scan.ranges.rbegin(); it != scan.ranges.rend(); ++it) { - readings.push_back(*it); + readings.push_back(karto::math::InRange(*it, range_min, range_max) ? *it : NAN); } } else { for (std::vector::const_iterator it = scan.ranges.begin(); it != scan.ranges.end(); ++it) { - readings.push_back(*it); + readings.push_back(karto::math::InRange(*it, range_min, range_max) ? *it : NAN); } }