Return if range it too small

CppStyleGuide
milvusrobotics · Jun 21, 2021 · 7330a13 · 7330a13
1 parent cf7af51
commit 7330a13
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/speckle_filter.cpp b/src/speckle_filter.cpp
@@ -76,11 +76,14 @@ bool LaserScanSpeckleFilter::update(const sensor_msgs::LaserScan& input_scan, se
   output_scan = input_scan;
   std::vector<bool> valid_ranges(output_scan.ranges.size(), false);
 
-  /*Check if range size is big enough to set the window */
-  size_t maxIdx = output_scan.ranges.size();
-  if(maxIdx >  config_.filter_window  && config_.filter_window  > 0) maxIdx -= (config_.filter_window-1);
+  /*Check if range size is big enough to use the filter window */
+  if(output_scan.ranges.size() <= config_.filter_window +1)
+  {
+      ROS_ERROR("Scanner range size is too small: size = %i", output_scan.ranges.size());
+      return false;
+  }
 
-  for (size_t idx = 0; idx < maxIdx; ++idx)
+  for (size_t idx = 0; idx < output_scan.ranges.size() - config_.filter_window + 1; ++idx)
   {
     bool window_valid = validator_->checkWindowValid(
           output_scan, idx, config_.filter_window, config_.max_range_difference);