From f640867ba3e06a2e4b2bc777deaaf058429b7885 Mon Sep 17 00:00:00 2001
From: HViktorTsoi <cuijiahetony@live.com>
Date: Mon, 9 Aug 2021 23:34:22 +0800
Subject: [PATCH] limit scan range

---
 src/laserMapping.cpp | 1 +
 src/preprocess.cpp   | 5 ++++-
 src/preprocess.h     | 1 +
 3 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp
index 2e1ab5f..fb36e96 100644
--- a/src/laserMapping.cpp
+++ b/src/laserMapping.cpp
@@ -732,6 +732,7 @@ int main(int argc, char** argv)
     nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001);
     nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001);
     nh.param<double>("preprocess/blind", p_pre->blind, 0.01);
+    nh.param<double>("preprocess/max_range",p_pre->max_scan_range, 100.f);
     nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA);
     nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
     nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
diff --git a/src/preprocess.cpp b/src/preprocess.cpp
index 9d1998a..51613fa 100644
--- a/src/preprocess.cpp
+++ b/src/preprocess.cpp
@@ -154,7 +154,10 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
               || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)
               && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind))
           {
-            pl_surf.push_back(pl_full[i]);
+            if(pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y < max_scan_range * max_scan_range){
+                pl_surf.push_back(pl_full[i]);
+            }
+//            pl_surf.push_back(pl_full[i]);
           }
         }
       }
diff --git a/src/preprocess.h b/src/preprocess.h
index 28303be..6ef6bb9 100644
--- a/src/preprocess.h
+++ b/src/preprocess.h
@@ -96,6 +96,7 @@ class Preprocess
   vector<orgtype> typess[128]; //maximum 128 line lidar
   int lidar_type, point_filter_num, N_SCANS;;
   double blind;
+  double max_scan_range;
   bool feature_enabled, given_offset_time;
   ros::Publisher pub_full, pub_surf, pub_corn;