added fix for all-nan messages on boot

Author: osullivan-locus

urg_node/include/uam/uam_driver_ros.h

@@ -204,6 +204,7 @@ class UamROS
         nullptr;
 
     std::vector<std::pair<size_t, float>> ranges_in_safety_area;
+    size_t finite_ranges_count = 0;
 
     for (size_t idx = 0; idx < reply.ranges.size(); idx++)
     {
@@ -217,6 +218,10 @@ class UamROS
       if (within_fov)
       {
         msg.ranges.push_back(range_m);
+        if (!std::isnan(range_m))
+        {
+          finite_ranges_count++;
+        }
       }
 
       if (safety_area != nullptr && !std::isnan(range_m))
@@ -229,6 +234,16 @@ class UamROS
       }
     }
 
+    if (finite_ranges_count == 0) {
+      // This driver/lidar combination will periodically boot up and publish all nan ranges for a moment.
+      // If this happens after a lidar power cycle (when the rest of the system is initilized), it breaks the
+      // laser filtering pipeline. The likleyhood of nominally receiving all nan readings in our usecase is so
+      // infinitesimally small we can just skip publishing in this case.
+      ROS_WARN_STREAM_THROTTLE(5.0, "All ranges are infinite, skipping scan publish!");
+      // We return early to avoid publishing mis-leading scans or area information.
+      return;
+    }
+
     if constexpr (is_detected<detected_intensities, T>::value)
     {
       msg.intensities.reserve(number_of_readings);