fix: make nan utls generic

Author: danhennage

include/am_utils/am_pcl_utils.h

@@ -14,6 +14,8 @@ class AMPCLUtils
   static pcl::PointCloud<pcl::PointXYZIR>::ConstPtr convertToPointXYZIR(
       const sensor_msgs::PointCloud2::ConstPtr pc2,
       pcl::PointCloud<pcl::PointXYZIR>::Ptr &pcl);
+  static bool isSetToNaN(pcl::PointXYZIR &point);
+  static void setNaN(pcl::PointXYZIR &point);
 };
 
 }

include/am_utils/dust_filter.h

@@ -34,8 +34,6 @@ class DustFilter
   float outside_min_z_m_;
   float outside_max_z_m_;
 
-  bool isSetToNaN(pcl::PointXYZIR &point);
-  void setNaN(pcl::PointXYZIR &point);
   void printInsideParams();
   void printOutsideParams();
 

src/am_utils/am_pcl_utils.cpp

@@ -31,5 +31,25 @@ pcl::PointCloud<pcl::PointXYZIR>::ConstPtr AMPCLUtils::convertToPointXYZIR(
   return pcl;
 }
 
+bool AMPCLUtils::isSetToNaN(pcl::PointXYZIR &point)
+{
+  if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) ||
+      std::isnan(point.intensity) || std::isnan(point.intensity))
+  {
+    setNaN(point);
+    return true;
+  }
+  return false;
+}
+
+void AMPCLUtils::setNaN(pcl::PointXYZIR &point)
+{
+  point.x = std::nanf("");
+  point.y = std::nanf("");
+  point.z = std::nanf("");
+  point.intensity = std::nanf("");
+  point.range = std::nanf("");
+}
+
 } // namespace
 

src/am_utils/dust_filter.cpp

@@ -3,6 +3,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl_ros/point_cloud.h>
 
+#include <am_utils/am_pcl_utils.h>
 #include <am_utils/dust_filter.h>
 #include <am_utils/point_types.h>
 #include <vb_util_lib/frame_names.h>
@@ -91,7 +92,7 @@ int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &pcl, bool skip_dust_fi
       pcl::PointXYZIR &point = pcl.points[index];
 
       // if already nan, continue
-      if(isSetToNaN(point))
+      if(AMPCLUtils::isSetToNaN(point))
       {
         nan_mask[index] = true;
         continue;
@@ -175,7 +176,7 @@ int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &pcl, bool skip_dust_fi
       pcl::PointXYZIR &point = pcl.points[index];
       if(nan_mask[index])
       {
-        setNaN(point);
+        AMPCLUtils::setNaN(point);
         points_removed++;
         is_dense = false;
       }
@@ -192,26 +193,6 @@ int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &pcl, bool skip_dust_fi
   return points_removed;
 }
 
-bool DustFilter::isSetToNaN(pcl::PointXYZIR &point)
-{
-  if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) ||
-      std::isnan(point.intensity) || std::isnan(point.intensity))
-  {
-    setNaN(point);
-    return true;
-  }
-  return false;
-}
-
-void DustFilter::setNaN(pcl::PointXYZIR &point)
-{
-  point.x = std::nanf("");
-  point.y = std::nanf("");
-  point.z = std::nanf("");
-  point.intensity = std::nanf("");
-  point.range = std::nanf("");
-}
-
 void DustFilter::setInsideFilter(float min_x_m_, float max_x_m_, float min_y_m_, float max_y_m_, float min_z_m_, float max_z_m_)
 {
   inside_min_x_m_ = min_x_m_;