12345qiupeng
/
Fast-Lio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add the path publish switch and new pcd save mode

main
xw 2021-08-16 12:13:18 -04:00
parent 53ee209c12
commit dcc38885ac
9 changed files with 60 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
config/avia.yaml
View File

@ -22,6 +22,12 @@ mapping:
0, 0, 1]
publish:
path_en: false
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: true
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

6
config/horizon.yaml
View File

@ -22,6 +22,12 @@ mapping:
0, 0, 1]
publish:
path_en: false
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: true
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

6
config/ouster64.yaml
View File

@ -22,6 +22,12 @@ mapping:
0, 0, 1]
publish:
path_en: false
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: true
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

6
config/velodyne.yaml
View File

@ -22,6 +22,12 @@ mapping:
0, 0, 1]
publish:
path_en: false
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: true
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

3
launch/mapping_avia.launch
View File

@ -6,13 +6,12 @@
<rosparam command="load" file="$(find fast_lio)/config/avia.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="1"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<group if="$(arg rviz)">

3
launch/mapping_horizon.launch
View File

@ -7,12 +7,11 @@
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="dense_publish_enable" type="bool" value="1" />
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<group if="$(arg rviz)">

1
launch/mapping_ouster64.launch
View File

@ -12,7 +12,6 @@
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<group if="$(arg rviz)">

1
launch/mapping_velodyne.launch
View File

@ -12,7 +12,6 @@
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<group if="$(arg rviz)">

35
src/laserMapping.cpp
View File

@ -70,7 +70,7 @@ double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_ti
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN];
double match_time = 0, solve_time = 0, solve_const_H_time = 0;
int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;
bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true;
bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true, path_en = true;
/**************************/
float res_last[100000] = {0.0};
@ -89,7 +89,7 @@ double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001;
double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0;
double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;
int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0;
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, pcd_save_interval = -1, pcd_index = 0;
bool point_selected_surf[100000] = {0};
bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;
bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
@ -479,7 +479,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
/* 2. noted that pcd save will influence the real-time performences **/
if (pcd_save_en)
{
int size = feats_undistort->points.size();
@ -492,6 +492,19 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
&laserCloudWorld->points[i]);
}
*pcl_wait_save += *laserCloudWorld;
static int scan_wait_num = 0;
scan_wait_num ++;
if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval)
{
pcd_index ++;
string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << all_points_dir << endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
pcl_wait_save->clear();
scan_wait_num = 0;
}
}
}
@ -717,9 +730,10 @@ int main(int argc, char** argv)
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
nh.param<bool>("publish/dense_publish_en",dense_pub_en,1);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en,1);
nh.param<bool>("publish/path_en",path_en, true);
nh.param<bool>("publish/scan_publish_en",scan_pub_en, true);
nh.param<bool>("publish/dense_publish_en",dense_pub_en, true);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
nh.param<string>("map_file_path",map_file_path,"");
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
@ -739,10 +753,11 @@ int main(int argc, char** argv)
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);
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, 0);
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false);
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("pcd_save_enable", pcd_save_en, 0);
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, 1);
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true);
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());
nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
@ -926,7 +941,7 @@ int main(int argc, char** argv)
t5 = omp_get_wtime();
/******* Publish points *******/
publish_path(pubPath);
if (path_en) publish_path(pubPath);
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull);
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);
// publish_effect_world(pubLaserCloudEffect);