PublisherLaserMap discarding nan and inf ranges

src/publisher_laser_map.cpp

@@ -187,18 +187,22 @@ void PublisherLaserMap::storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPt
     Eigen::ArrayXf orientations = Eigen::ArrayXf::LinSpaced(ranges.size(),
                                                             scan_data.params_.angle_min_ + laser_extrinsics_o,
                                                             scan_data.params_.angle_max_ + laser_extrinsics_o);
+    // filter NaN or Inf
+    Eigen::ArrayXf ranges_filtered = ranges.isFinite().select(ranges, 0.0);
+    if (discard_max_range_)
+        ranges_filtered = (ranges_filtered < scan_data.params_.range_max_-1e-3).select(ranges_filtered, 0.0);
 
+    // compute cartesian discarding ranges
     scan_data.local_points_.resize(2,ranges.size());
-    if (discard_max_range_)
-    {
-        scan_data.local_points_.row(0) = orientations.cos() * (ranges < scan_data.params_.range_max_ -0.1).select(ranges, 0) + laser_extrinsics_p(0);
-        scan_data.local_points_.row(1) = orientations.sin() * (ranges < scan_data.params_.range_max_ -0.1).select(ranges, 0) + laser_extrinsics_p(1);
-    }
-    else
-    {
-        scan_data.local_points_.row(0) = orientations.cos() * ranges + laser_extrinsics_p(0);
-        scan_data.local_points_.row(1) = orientations.sin() * ranges + laser_extrinsics_p(1);
-    }
+    scan_data.local_points_.row(0) = orientations.cos() * ranges_filtered + laser_extrinsics_p(0);
+    scan_data.local_points_.row(1) = orientations.sin() * ranges_filtered + laser_extrinsics_p(1);
+    
+    // // discard max range (optionally)
+    // if (discard_max_range_)
+    // {
+    //     scan_data.local_points_.row(0) = (ranges < scan_data.params_.range_max_-1e-3).select(scan_data.local_points_.row(0), 0.0);
+    //     scan_data.local_points_.row(1) = (ranges < scan_data.params_.range_max_-1e-3).select(scan_data.local_points_.row(1), 0.0);
+    // }
 
     // local pointcloud
     scan_data.local_pc_ = pcl::PointCloud<pcl::PointXYZRGB>(ranges.size(),