mirrors/FAST_LIO
1
0
Fork 0
mirror of https://github.com/hku-mars/FAST_LIO.git synced 2023-04-06 16:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

add the path publish switch and new pcd save mode

Browse Source
This commit is contained in:
xw
2021-08-16 12:13:18 -04:00
parent 53ee209c12
commit dcc38885ac
9 changed files with 60 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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);