From 99c462a36bf11485a0a259b658e56ab00fbb581a Mon Sep 17 00:00:00 2001 From: zfc-zfc <625257073@qq.com> Date: Wed, 24 Aug 2022 21:06:01 +0800 Subject: [PATCH] Add new parameter: time offset between LiDAR and IMU. --- config/avia.yaml | 4 +++- config/horizon.yaml | 4 +++- config/ouster64.yaml | 2 ++ config/velodyne.yaml | 4 +++- src/laserMapping.cpp | 4 ++++ 5 files changed, 15 insertions(+), 3 deletions(-) diff --git a/config/avia.yaml b/config/avia.yaml index a98aa4eb..adcc03ba 100644 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -2,6 +2,8 @@ common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 preprocess: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, @@ -30,4 +32,4 @@ publish: 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. \ No newline at end of file + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/horizon.yaml b/config/horizon.yaml index b7bc6372..1c489c54 100644 --- a/config/horizon.yaml +++ b/config/horizon.yaml @@ -2,6 +2,8 @@ common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 preprocess: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, @@ -30,4 +32,4 @@ publish: 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. \ No newline at end of file + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/ouster64.yaml b/config/ouster64.yaml index 3e6ef566..11d16ae3 100644 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -2,6 +2,8 @@ common: lid_topic: "/os_cloud_node/points" imu_topic: "/os_cloud_node/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 preprocess: lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, diff --git a/config/velodyne.yaml b/config/velodyne.yaml index b5076b54..453777be 100644 --- a/config/velodyne.yaml +++ b/config/velodyne.yaml @@ -2,7 +2,9 @@ common: lid_topic: "/velodyne_points" imu_topic: "/imu/data" time_sync_en: false # ONLY turn on when external time synchronization is really not possible - + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + preprocess: lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index a9632c4b..15935fb5 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -76,6 +76,7 @@ bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extri float res_last[100000] = {0.0}; float DET_RANGE = 300.0f; const float MOV_THRESHOLD = 1.5f; +double time_diff_lidar_to_imu = 0.0; mutex mtx_buffer; condition_variable sig_buffer; @@ -343,6 +344,8 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec()); } + msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_diff_lidar_to_imu); + double timestamp = msg->header.stamp.toSec(); mtx_buffer.lock(); @@ -760,6 +763,7 @@ int main(int argc, char** argv) nh.param("common/lid_topic",lid_topic,"/livox/lidar"); nh.param("common/imu_topic", imu_topic,"/livox/imu"); nh.param("common/time_sync_en", time_sync_en, false); + nh.param("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); nh.param("filter_size_corner",filter_size_corner_min,0.5); nh.param("filter_size_surf",filter_size_surf_min,0.5); nh.param("filter_size_map",filter_size_map_min,0.5);