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("mapping/b_gyr_cov",b_gyr_cov,0.0001); nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); nh.param("preprocess/blind", p_pre->blind, 0.01); + nh.param("preprocess/max_range",p_pre->max_scan_range, 100.f); nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); nh.param("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 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;