admin/Point-LIO_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Support ros2 Humble.

Browse Source
This commit is contained in:
uavfly
2025-10-28 18:28:57 +08:00
parent 97b0042e39
commit 5dcf0c4c26
23 changed files with 1432 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
CMakeLists.txt
View File

@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.5)
project(point_lio)
SET(CMAKE_BUILD_TYPE "Debug")
@@ -15,26 +15,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
include(ProcessorCount)
ProcessorCount(N)
message("Processer number: ${N}")
if(N GREATER 5)
add_definitions(-DMP_EN)
add_definitions(-DMP_PROC_NUM=4)
message("core for MP: 3")
elseif(N GREATER 3)
math(EXPR PROC_NUM "${N} - 2")
add_definitions(-DMP_EN)
add_definitions(-DMP_PROC_NUM="${PROC_NUM}")
message("core for MP: ${PROC_NUM}")
else()
add_definitions(-DMP_PROC_NUM=1)
endif()
else()
add_definitions(-DMP_PROC_NUM=1)
endif()
#add_compile_definitions(BOOST_BIND_GLOBAL_PLACEHOLDERS)
find_package(OpenMP QUIET)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
@@ -43,22 +24,23 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
find_package(PythonLibs REQUIRED)
find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
pcl_ros
tf
livox_ros_driver
message_generation
eigen_conversions
)
# Find packages
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(livox_ros_driver2 REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(PCL 1.8 REQUIRED)
find_package(PCL REQUIRED)
message(Eigen: ${EIGEN3_INCLUDE_DIR})
@@ -74,22 +56,36 @@ link_directories(
${PCL_LIBRARY_DIRS}
)
generate_messages(
DEPENDENCIES
geometry_msgs
# Declare a ROS2 executable
add_executable(pointlio_mapping src/laserMapping.cpp src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp src/IMU_Processing.cpp)
ament_target_dependencies(pointlio_mapping
rclcpp
rclpy
geometry_msgs
nav_msgs
sensor_msgs
pcl_ros
pcl_conversions
tf2_ros
tf2_eigen
livox_ros_driver2
visualization_msgs
)
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
DEPENDS EIGEN3 PCL
INCLUDE_DIRS
)
add_executable(pointlio_mapping src/laserMapping.cpp
src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp
src/IMU_Processing.cpp)
target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_link_libraries(pointlio_mapping ${PYTHON_LIBRARIES})
target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
# Install the executable
install(TARGETS
pointlio_mapping
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY config launch rviz_cfg
DESTINATION share/${PROJECT_NAME}
)
# Export dependencies
ament_export_dependencies(rclcpp rclpy geometry_msgs nav_msgs sensor_msgs pcl_ros pcl_conversions tf2_ros tf2_eigen livox_ros_driver2 Eigen3)
ament_package()

111
config/avia.yaml
View File

@@ -1,54 +1,67 @@
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
/**:
ros__parameters:
use_imu_as_input: True # Change to True to use IMU as input of Point-LIO
prop_at_freq_of_imu: True
check_satu: True
init_map_size: 1000
point_filter_num: 3 # Options: 1, 3
space_down_sample: True
filter_size_surf: 0.5 # Options: 0.5, 0.3, 0.2, 0.15, 0.1
filter_size_map: 0.5 # Options: 0.5, 0.3, 0.15, 0.1
cube_side_length: 1000.0 # Option: 1000
runtime_pos_log_enable: false # Option: True
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 1 # 4
scan_line: 6 # 32
timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 0.5
preprocess:
lidar_type: 1 # 4
scan_line: 6 # 32
timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 0.5
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 9.810 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001; 0.01
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 0.1
imu_meas_omg_cov: 0.1 #0.01 # 0.1
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins #
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 9.810 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001; 0.01
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 0.1
imu_meas_omg_cov: 0.1 #0.01 # 0.1
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins #
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
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.
pcd_save:
pcd_save_en: false
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.

111
config/horizon.yaml
View File

@@ -1,54 +1,67 @@
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
/**:
ros__parameters:
use_imu_as_input: True # Change to True to use IMU as input of Point-LIO
prop_at_freq_of_imu: True
check_satu: True
init_map_size: 1000
point_filter_num: 3 # Options: 1, 3
space_down_sample: True
filter_size_surf: 0.5 # Options: 0.5, 0.3, 0.2, 0.15, 0.1
filter_size_map: 0.5 # Options: 0.5, 0.3, 0.15, 0.1
cube_side_length: 1000.0 # Option: 1000
runtime_pos_log_enable: false # Option: True
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 1
scan_line: 6
timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 4.0
preprocess:
lidar_type: 1
scan_line: 6
timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 4.0
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.01 #0.1 # 2
imu_meas_omg_cov: 0.01 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.01 #0.1 # 2
imu_meas_omg_cov: 0.01 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
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.
pcd_save:
pcd_save_en: false
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.

67
config/mid360.yaml Normal file
View File

@@ -0,0 +1,67 @@
/**:
ros__parameters:
use_imu_as_input: True # Change to True to use IMU as input of Point-LIO
prop_at_freq_of_imu: True
check_satu: True
init_map_size: 1000
point_filter_num: 3 # Options: 1, 3
space_down_sample: True
filter_size_surf: 0.5 # Options: 0.5, 0.3, 0.2, 0.15, 0.1
filter_size_map: 0.5 # Options: 0.5, 0.3, 0.15, 0.1
cube_side_length: 1000.0 # Option: 1000
runtime_pos_log_enable: false # Option: True
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR
scan_line: 4
timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 0.5
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 9.810 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001; 0.01
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 0.1
imu_meas_omg_cov: 0.1 #0.01 # 0.1
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins #
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
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.

111
config/ouster64.yaml
View File

@@ -1,54 +1,67 @@
common:
lid_topic: "/os_cloud_node/points"
imu_topic: "/os_cloud_node/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
/**:
ros__parameters:
use_imu_as_input: True # Change to True to use IMU as input of Point-LIO
prop_at_freq_of_imu: True
check_satu: True
init_map_size: 1000
point_filter_num: 3 # Options: 1, 3
space_down_sample: True
filter_size_surf: 0.5 # Options: 0.5, 0.3, 0.2, 0.15, 0.1
filter_size_map: 0.5 # Options: 0.5, 0.3, 0.15, 0.1
cube_side_length: 1000.0 # Option: 1000
runtime_pos_log_enable: false # Option: True
common:
lid_topic: "/os_cloud_node/points"
imu_topic: "/os_cloud_node/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 3 # 2 #velodyne # 1 Livox Avia LiDAR
scan_line: 64 # 32 #velodyne 6 avia
timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 4.0
preprocess:
lidar_type: 3 # 2 #velodyne # 1 Livox Avia LiDAR
scan_line: 64 # 32 #velodyne 6 avia
timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 4.0
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.01 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 2
imu_meas_omg_cov: 0.1 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [0.0, 0.0, 0.0]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.01 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 2
imu_meas_omg_cov: 0.1 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [0.0, 0.0, 0.0]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
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.
pcd_save:
pcd_save_en: false
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.

123
config/velody16.yaml
View File

@@ -1,60 +1,73 @@
common:
lid_topic: "/velodyne_points"
imu_topic: "/imu/data"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
/**:
ros__parameters:
use_imu_as_input: True # Change to True to use IMU as input of Point-LIO
prop_at_freq_of_imu: True
check_satu: True
init_map_size: 1000
point_filter_num: 3 # Options: 1, 3
space_down_sample: True
filter_size_surf: 0.5 # Options: 0.5, 0.3, 0.2, 0.15, 0.1
filter_size_map: 0.5 # Options: 0.5, 0.3, 0.15, 0.1
cube_side_length: 1000.0 # Option: 1000
runtime_pos_log_enable: false # Option: True
common:
lid_topic: "/velodyne_points"
imu_topic: "/imu/data"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 2
scan_line: 32
timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 2.0
preprocess:
lidar_type: 2
scan_line: 32
timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 2.0
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.01 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 2
imu_meas_omg_cov: 0.1 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [-0.30, 0.880, -9.76] # liosam [0.0, 9.810, 0.0] # # preknown gravity, use when imu_en is false or start from a non-stationary state
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm
# extrinsic_R: [ 0, 1, 0,
# -1, 0, 0,
# 0, 0, 1 ] # ulhk 5 6
# extrinsic_R: [ 0, -1, 0,
# 1, 0, 0,
# 0, 0, 1 ] # utbm 1, 2
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ] # ulhk 4 utbm 3
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.01 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 2
imu_meas_omg_cov: 0.1 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [-0.30, 0.880, -9.76] # liosam [0.0, 9.810, 0.0] # # preknown gravity, use when imu_en is false or start from a non-stationary state
gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU
extrinsic_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm
# extrinsic_R: [ 0, 1, 0,
# -1, 0, 0,
# 0, 0, 1 ] # ulhk 5 6
# extrinsic_R: [ 0, -1, 0,
# 1, 0, 0,
# 0, 0, 1 ] # utbm 1, 2
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ] # ulhk 4 utbm 3
odometry:
publish_odometry_without_downsample: false
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
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.
pcd_save:
pcd_save_en: false
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.

23
include/common_lib.h
View File

@@ -5,10 +5,10 @@
#include <Eigen/Eigen>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
#include <queue>
@@ -121,7 +121,7 @@ struct MeasureGroup // Lidar data and imu dates for the curent process
double lidar_beg_time;
double lidar_last_time;
PointCloudXYZI::Ptr lidar;
deque<sensor_msgs::Imu::ConstPtr> imu;
deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu;
};
template <typename T>
@@ -236,4 +236,17 @@ bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &
// template<typename T>
// const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
inline double get_time_sec(const builtin_interfaces::msg::Time &time)
{
return rclcpp::Time(time).seconds();
}
inline rclcpp::Time get_ros_time(double timestamp)
{
int32_t sec = std::floor(timestamp);
auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9;
uint32_t nanosec = nanosec_d;
return rclcpp::Time(sec, nanosec);
}
#endif

68
launch/mapping_avia.launch.py Normal file
View File

@@ -0,0 +1,68 @@
from launch import LaunchDescription
from launch.actions import GroupAction, DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare the RViz argument
rviz_arg = DeclareLaunchArgument(
'rviz', default_value='true',
description='Flag to launch RViz.')
# Node parameters, including those from the YAML configuration file
laser_mapping_params = [
PathJoinSubstitution([
FindPackageShare('point_lio'),
'config', 'avia.yaml'
]),
{
'use_imu_as_input': False, # Change to True to use IMU as input of Point-LIO
'prop_at_freq_of_imu': True,
'check_satu': True,
'init_map_size': 10,
'point_filter_num': 1, # options: 4, 3
'space_down_sample': True,
'filter_size_surf': 0.3, # options: 0.5, 0.3, 0.2, 0.15, 0.1
'filter_size_map': 0.2, # options: 0.5, 0.3, 0.15, 0.1
'cube_side_length': 2000.0, # option: 1000
'runtime_pos_log_enable': False, # option: True
},
]
# Node definition for laserMapping with Point-LIO
laser_mapping_node = Node(
package='point_lio',
executable='pointlio_mapping',
name='laserMapping',
output='screen',
parameters=laser_mapping_params,
# prefix='gdb -ex run --args'
)
# Conditional RViz node launch
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', PathJoinSubstitution([
FindPackageShare('point_lio'),
'rviz_cfg', 'loam_livox.rviz'
])],
condition=IfCondition(LaunchConfiguration('rviz')),
prefix='nice'
)
# Assemble the launch description
ld = LaunchDescription([
rviz_arg,
laser_mapping_node,
GroupAction(
actions=[rviz_node],
condition=IfCondition(LaunchConfiguration('rviz'))
),
])
return ld

68
launch/mapping_horizon.launch.py Normal file
View File

@@ -0,0 +1,68 @@
from launch import LaunchDescription
from launch.actions import GroupAction, DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare the RViz argument
rviz_arg = DeclareLaunchArgument(
'rviz', default_value='true',
description='Flag to launch RViz.')
# Node parameters, including those from the YAML configuration file
laser_mapping_params = [
PathJoinSubstitution([
FindPackageShare('point_lio'),
'config', 'horizon.yaml'
]),
{
'use_imu_as_input': False, # Change to True to use IMU as input of Point-LIO
'prop_at_freq_of_imu': True,
'check_satu': True,
'init_map_size': 10,
'point_filter_num': 3, # Options: 1, 3
'space_down_sample': True,
'filter_size_surf': 0.5, # Options: 0.5, 0.3, 0.2, 0.15, 0.1
'filter_size_map': 0.5, # Options: 0.5, 0.3, 0.15, 0.1
'cube_side_length': 1000.0, # Option: 1000
'runtime_pos_log_enable': False, # Option: True
}
]
# Node definition for laserMapping with Point-LIO
laser_mapping_node = Node(
package='point_lio',
executable='pointlio_mapping',
name='laserMapping',
output='screen',
parameters=laser_mapping_params,
# prefix='gdb -ex run --args'
)
# Conditional RViz node launch
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', PathJoinSubstitution([
FindPackageShare('point_lio'),
'rviz_cfg', 'loam_livox.rviz'
])],
condition=IfCondition(LaunchConfiguration('rviz')),
prefix='nice'
)
# Assemble the launch description
ld = LaunchDescription([
rviz_arg,
laser_mapping_node,
GroupAction(
actions=[rviz_node],
condition=IfCondition(LaunchConfiguration('rviz'))
),
])
return ld

57
launch/mapping_mid360.launch.py Normal file
View File

@@ -0,0 +1,57 @@
from launch import LaunchDescription
from launch.actions import GroupAction, DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare the RViz argument
rviz_arg = DeclareLaunchArgument(
'rviz', default_value='true',
description='Flag to launch RViz.')
# Node parameters, including those from the YAML configuration file
laser_mapping_params = [
PathJoinSubstitution([
FindPackageShare('point_lio'),
'config', 'mid360.yaml'
])
]
# Node definition for laserMapping with Point-LIO
laser_mapping_node = Node(
package='point_lio',
executable='pointlio_mapping',
name='laserMapping',
output='screen',
parameters=laser_mapping_params,
# prefix='gdb -ex run --args'
)
# Conditional RViz node launch
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', PathJoinSubstitution([
FindPackageShare('point_lio'),
'rviz_cfg', 'loam_livox.rviz'
])],
condition=IfCondition(LaunchConfiguration('rviz')),
prefix='nice'
)
# Assemble the launch description
ld = LaunchDescription([
rviz_arg,
laser_mapping_node,
#octomap_server_node,
GroupAction(
actions=[rviz_node],
condition=IfCondition(LaunchConfiguration('rviz'))
),
])
return ld

68
launch/mapping_ouster64.launch.py Normal file
View File

@@ -0,0 +1,68 @@
from launch import LaunchDescription
from launch.actions import GroupAction, DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare the RViz argument
rviz_arg = DeclareLaunchArgument(
'rviz', default_value='true',
description='Flag to launch RViz.')
# Node parameters, including those from the YAML configuration file
laser_mapping_params = [
PathJoinSubstitution([
FindPackageShare('point_lio'),
'config', 'ouster64.yaml'
]),
{
'use_imu_as_input': False, # Change to True to use IMU as input of Point-LIO
'prop_at_freq_of_imu': True,
'check_satu': True,
'init_map_size': 10,
'point_filter_num': 4, # Options: 4, 3
'space_down_sample': True,
'filter_size_surf': 0.5, # Options: 0.5, 0.3, 0.2, 0.15, 0.1
'filter_size_map': 0.5, # Options: 0.5, 0.3, 0.15, 0.1
'cube_side_length': 1000.0, # Option: 1000 (changed from 2000)
'runtime_pos_log_enable': False, # Option: True
}
]
# Node definition for laserMapping with Point-LIO
laser_mapping_node = Node(
package='point_lio',
executable='pointlio_mapping',
name='laserMapping',
output='screen',
parameters=laser_mapping_params,
# prefix='gdb -ex run --args'
)
# Conditional RViz node launch
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', PathJoinSubstitution([
FindPackageShare('point_lio'),
'rviz_cfg', 'loam_livox.rviz'
])],
condition=IfCondition(LaunchConfiguration('rviz')),
prefix='nice'
)
# Assemble the launch description
ld = LaunchDescription([
rviz_arg,
laser_mapping_node,
GroupAction(
actions=[rviz_node],
condition=IfCondition(LaunchConfiguration('rviz'))
),
])
return ld

68
launch/mapping_velody16.launch.py Normal file
View File

@@ -0,0 +1,68 @@
from launch import LaunchDescription
from launch.actions import GroupAction, DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare the RViz argument
rviz_arg = DeclareLaunchArgument(
'rviz', default_value='true',
description='Flag to launch RViz.')
# Node parameters, including those from the YAML configuration file
laser_mapping_params = [
PathJoinSubstitution([
FindPackageShare('point_lio'),
'config', 'velody16.yaml'
]),
{
'use_imu_as_input': False, # Change to True to use IMU as input of Point-LIO
'prop_at_freq_of_imu': True,
'check_satu': True,
'init_map_size': 10,
'point_filter_num': 4, # Options: 4, 3
'space_down_sample': True,
'filter_size_surf': 0.5, # Options: 0.5, 0.3, 0.2, 0.15, 0.1
'filter_size_map': 0.5, # Options: 0.5, 0.3, 0.15, 0.1
'cube_side_length': 1000.0, # Option: 1000 (changed from 2000)
'runtime_pos_log_enable': False, # Option: True
}
]
# Node definition for laserMapping with Point-LIO
laser_mapping_node = Node(
package='point_lio',
executable='pointlio_mapping',
name='laserMapping',
output='screen',
parameters=laser_mapping_params,
# prefix='gdb -ex run --args'
)
# Conditional RViz node launch
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', PathJoinSubstitution([
FindPackageShare('point_lio'),
'rviz_cfg', 'loam_livox.rviz'
])],
condition=IfCondition(LaunchConfiguration('rviz')),
prefix='nice'
)
# Assemble the launch description
ld = LaunchDescription([
rviz_arg,
laser_mapping_node,
GroupAction(
actions=[rviz_node],
condition=IfCondition(LaunchConfiguration('rviz'))
),
])
return ld

43
package.xml
View File

@@ -1,5 +1,5 @@
<?xml version="1.0"?>
<package>
<package format="3">
<name>point_lio</name>
<version>0.0.0</version>
@@ -16,32 +16,25 @@
<author email="hdj65822@connect.hku.hk">Dongjiao He</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>livox_ros_driver</build_depend>
<build_depend>message_generation</build_depend>
<!-- ROS2 uses ament as build system -->
<buildtool_depend>ament_cmake</buildtool_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>livox_ros_driver</run_depend>
<run_depend>message_runtime</run_depend>
<!-- Dependencies are now also categorized as build, build_export, and exec_depend -->
<depend>rclcpp</depend> <!-- roscpp in ROS1 is replaced by rclcpp in ROS2 -->
<depend>rclpy</depend> <!-- rospy is replaced by rclpy in ROS2 -->
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>tf2_ros</depend> <!-- tf in ROS1 is replaced by tf2 in ROS2 -->
<depend>pcl_ros</depend> <!-- For PCL support -->
<depend>pcl_conversions</depend> <!-- For PCL support -->
<depend>livox_ros_driver2</depend>
<test_depend>rostest</test_depend>
<test_depend>rosbag</test_depend>
<!-- test_depend remains the same, but make sure the testing tools you use are compatible with ROS2 -->
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>

490
rviz_cfg/loam_livox.rviz
View File

@@ -1,356 +1,318 @@
Panels:
- Class: rviz/Displays
Help Height: 0
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Axes1
- /mapping1
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/currPoints1/Autocompute Value Bounds1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Odometry1/Odometry1/Covariance1
- /Odometry1/Odometry1/Covariance1/Position1
- /Odometry1/Odometry1/Covariance1/Orientation1
- /MarkerArray1/Namespaces1
Splitter Ratio: 0.6432291865348816
Tree Height: 1297
- Class: rviz/Selection
- /Status1
Splitter Ratio: 0.5
Tree Height: 549
- Class: rviz_common/Selection
Name: Selection
- Class: rviz/Tool Properties
- Class: rviz_common/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
SyncSource: CloudRegistered
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1000
Class: rviz/Grid
Color: 160; 160; 164
Enabled: false
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 40
Reference Frame: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz/Axes
- Class: rviz_default_plugins/TF
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.05999999865889549
Reference Frame: body
Show Trail: false
Frame Timeout: 15
Frames:
All Enabled: true
aft_mapped:
Value: true
base_footprint:
Value: true
base_link:
Value: true
camera_init:
Value: true
camera_link:
Value: true
gyro_link:
Value: true
laser:
Value: true
left_front_link:
Value: true
left_wheel_link:
Value: true
map:
Value: true
odom_combined:
Value: true
right_front_link:
Value: true
right_wheel_link:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
camera_init:
aft_mapped:
{}
Update Interval: 0
Value: true
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 238; 238; 236
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 3
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1000
Enabled: true
Invert Rainbow: true
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: mapping
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.009999999776482582
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.20000000298023224
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /Odometry
Unreliable: false
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: Odometry
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /Odometry
Value: true
- Alpha: 0
Buffer Length: 2
Class: rviz/Path
Color: 25; 255; 255
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0
Head Length: 0
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.20000000298023224
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /path
Value: true
- Alpha: 1
Autocompute Intensity Bounds: false
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8584054708480835
Min Value: -0.10289539396762848
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 30
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 186
Min Color: 0; 0; 0
Min Intensity: 0
Name: CloudRegistered
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_registered
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Enabled: true
Invert Rainbow: false
Max Color: 239; 41; 41
Max Intensity: 0
Min Color: 239; 41; 41
Max Color: 255; 255; 255
Max Intensity: 184
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Name: CloudEffected
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 4
Size (m): 0.30000001192092896
Style: Spheres
Topic: /cloud_effected
Unreliable: false
Size (Pixels): 3
Size (m): 0.019999999552965164
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_effected
Use Fixed Frame: true
Use rainbow: true
Value: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 13.139549255371094
Min Value: -32.08251953125
Max Value: 2.036320447921753
Min Value: -0.09378375858068466
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 138; 226; 52
Color Transformer: FlatColor
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: false
Enabled: true
Invert Rainbow: false
Max Color: 138; 226; 52
Min Color: 138; 226; 52
Name: PointCloud2
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 0
Name: CloudMap
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Size (m): 0.019999999552965164
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /Laser_map
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /MarkerArray
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: camera_init
Frame Rate: 10
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic: /clicked_point
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 100.72154235839844
Class: rviz_default_plugins/Orbit
Distance: 28.403661727905273
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 34.07209014892578
Y: -2.53304386138916
Z: -8.43538761138916
X: -2.2604103088378906
Y: -0.3224470913410187
Z: 0.9725424647331238
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.06979652494192123
Target Frame: global
Yaw: 4.245370388031006
Pitch: 0.7497965693473816
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 1.7503817081451416
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1536
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001c80000054efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000054e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000052fc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007ea0000054e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -358,7 +320,7 @@ Window Geometry:
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 2488
X: 72
Y: 27
collapsed: false
Width: 1453
X: 2404
Y: 218

4
src/IMU_Processing.cpp
View File

@@ -27,7 +27,7 @@ ImuProcess::~ImuProcess() {}
void ImuProcess::Reset()
{
ROS_WARN("Reset ImuProcess");
RCLCPP_WARN(logger, "Reset ImuProcess");
mean_acc = V3D(0, 0, 0.0);
mean_gyr = V3D(0, 0, 0);
imu_need_init_ = true;
@@ -71,7 +71,7 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
RCLCPP_WARN(logger, "IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
V3D cur_acc, cur_gyr;
if (b_first_frame_)

10
src/IMU_Processing.h
View File

@@ -5,7 +5,7 @@
// #include <mutex>
// #include <thread>
#include <csignal>
#include <ros/ros.h>
#include <rclcpp/rclcpp.hpp>
// #include <so3_math.h>
#include <Eigen/Eigen>
// #include "Estimator.h"
@@ -13,12 +13,12 @@
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/msg/odometry.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
/// *************Preconfiguration

591
src/laserMapping.cpp
View File

@@ -1,19 +1,19 @@
// #include <so3_math.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include "li_initialization.h"
#include <malloc.h>
// #include <cv_bridge/cv_bridge.h>
// #include "matplotlibcpp.h"
// #include <ros/console.h>
using namespace std;
@@ -42,14 +42,14 @@ pcl::VoxelGrid<PointType> downSizeFilterMap;
V3D euler_cur;
nav_msgs::Path path;
nav_msgs::Odometry odomAftMapped;
geometry_msgs::PoseStamped msg_body_pose;
nav_msgs::msg::Path path;
nav_msgs::msg::Odometry odomAftMapped;
geometry_msgs::msg::PoseStamped msg_body_pose;
void SigHandle(int sig)
{
flg_exit = true;
ROS_WARN("catch sig %d", sig);
RCLCPP_WARN(logger, "catch sig %d", sig);
sig_buffer.notify_all();
}
@@ -149,22 +149,22 @@ void MapIncremental() {
ivox_->AddPoints(points_to_add);
}
void publish_init_map(const ros::Publisher & pubLaserCloudFullRes)
void publish_init_map(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pubLaserCloudFullRes)
{
int size_init_map = init_feats_world->size();
sensor_msgs::PointCloud2 laserCloudmsg;
sensor_msgs::msg::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*init_feats_world, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudmsg);
pubLaserCloudFullRes->publish(laserCloudmsg);
}
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
void publish_frame_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pubLaserCloudFullRes)
{
if (scan_pub_en)
{
@@ -180,12 +180,12 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
laserCloudWorld->points[i].z = feats_down_world->points[i].z;
laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; // feats_down_world->points[i].y; //
}
sensor_msgs::PointCloud2 laserCloudmsg;
sensor_msgs::msg::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudmsg);
pubLaserCloudFullRes->publish(laserCloudmsg);
// publish_count -= PUBFRAME_PERIOD;
}
@@ -222,7 +222,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
}
}
void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
void publish_frame_body(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pubLaserCloudFull_body)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
@@ -233,11 +233,11 @@ void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
&laserCloudIMUBody->points[i]);
}
sensor_msgs::PointCloud2 laserCloudmsg;
sensor_msgs::msg::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
laserCloudmsg.header.frame_id = "body";
pubLaserCloudFull_body.publish(laserCloudmsg);
pubLaserCloudFull_body->publish(laserCloudmsg);
// publish_count -= PUBFRAME_PERIOD;
}
@@ -268,62 +268,60 @@ void set_posestamp(T & out)
}
}
void publish_odometry(const ros::Publisher & pubOdomAftMapped)
void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pubOdomAftMapped)
{
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "body";
if (publish_odometry_without_downsample)
{
odomAftMapped.header.stamp = ros::Time().fromSec(time_current);
odomAftMapped.header.stamp = get_ros_time(time_current);
}
else
{
odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);
odomAftMapped.header.stamp = get_ros_time(lidar_end_time);
}
set_posestamp(odomAftMapped.pose.pose);
pubOdomAftMapped.publish(odomAftMapped);
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \
odomAftMapped.pose.pose.position.y, \
odomAftMapped.pose.pose.position.z));
q.setW(odomAftMapped.pose.pose.orientation.w);
q.setX(odomAftMapped.pose.pose.orientation.x);
q.setY(odomAftMapped.pose.pose.orientation.y);
q.setZ(odomAftMapped.pose.pose.orientation.z);
transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body") );
pubOdomAftMapped->publish(odomAftMapped);
geometry_msgs::msg::TransformStamped transform;
transform.header.frame_id = "camera_init";
transform.child_frame_id = "body";
transform.transform.translation.x = odomAftMapped.pose.pose.position.x;
transform.transform.translation.y = odomAftMapped.pose.pose.position.y;
transform.transform.translation.z = odomAftMapped.pose.pose.position.z;
transform.transform.rotation.w = odomAftMapped.pose.pose.orientation.w;
transform.transform.rotation.x = odomAftMapped.pose.pose.orientation.x;
transform.transform.rotation.y = odomAftMapped.pose.pose.orientation.y;
transform.transform.rotation.z = odomAftMapped.pose.pose.orientation.z;
transform.header.stamp = odomAftMapped.header.stamp;
tf_br->sendTransform(transform);
}
void publish_path(const ros::Publisher pubPath)
void publish_path(const rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr & pubPath)
{
set_posestamp(msg_body_pose.pose);
// msg_body_pose.header.stamp = ros::Time::now();
msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
// msg_body_pose.header.stamp = nh->get_clock()->now();
msg_body_pose.header.stamp = get_ros_time(lidar_end_time);
msg_body_pose.header.frame_id = "camera_init";
static int jjj = 0;
jjj++;
// if (jjj % 2 == 0) // if path is too large, the rvis will crash
{
path.poses.emplace_back(msg_body_pose);
pubPath.publish(path);
pubPath->publish(path);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh("~");
ros::AsyncSpinner spinner(0);
spinner.start();
rclcpp::init(argc, argv);
rclcpp::Node::SharedPtr nh = std::make_shared<rclcpp::Node>("laserMapping");
readParameters(nh);
cout<<"lidar_type: "<<lidar_type<<endl;
ivox_ = std::make_shared<IVoxType>(ivox_options_);
path.header.stamp = ros::Time().fromSec(lidar_end_time);
path.header.stamp = get_ros_time(lidar_end_time);
path.header.frame_id ="camera_init";
/*** variables definition for counting ***/
@@ -371,38 +369,41 @@ int main(int argc, char** argv)
open_file();
/*** ROS subscribe initialization ***/
ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pcl_pc_;
rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_pcl_livox_;
if (p_pre->lidar_type == AVIA) {
sub_pcl_livox_ = nh->create_subscription<livox_ros_driver2::msg::CustomMsg>(lid_topic, 200000, livox_pcl_cbk);
} else {
sub_pcl_pc_ = nh->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, 200000, standard_pcl_cbk);
}
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu = nh->create_subscription<sensor_msgs::msg::Imu>(imu_topic, 200000, imu_cbk);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 1000);
ros::Publisher pubLaserCloudFullRes_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 1000);
// ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
// ("/cloud_effected", 1000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 1000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 1000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered", 1000);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes_body = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered_body", 1000);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_effected", 1000);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 1000);
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped = nh->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 1000);
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath = nh->create_publisher<nav_msgs::msg::Path>("/path", 1000);
("/path", 1000);
// ros::Publisher plane_pub = nh.advertise<visualization_msgs::Marker>
// ("/planner_normal", 1000);
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub = nh->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1000);
tf2_ros::TransformBroadcaster::SharedPtr tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(nh);
//------------------------------------------------------------------------------------------------------
signal(SIGINT, SigHandle);
ros::Rate loop_rate(500);
bool status = ros::ok();
while (status)
rclcpp::Rate loop_rate(500);
while (rclcpp::ok())
{
if (flg_exit) break;
ros::spinOnce();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(nh);
executor.spin_some();
if(sync_packages(Measures))
{
if (flg_reset)
{
ROS_WARN("reset when rosbag play back");
RCLCPP_WARN(logger, "reset when rosbag play back");
p_imu->Reset();
feats_undistort.reset(new PointCloudXYZI());
if (use_imu_as_input)
@@ -433,7 +434,7 @@ int main(int argc, char** argv)
flg_first_scan = false;
if (first_imu_time < 1)
{
first_imu_time = imu_next.header.stamp.toSec();
first_imu_time = get_time_sec(imu_next.header.stamp);
printf("first imu time: %f\n", first_imu_time);
}
time_current = 0.0;
@@ -446,7 +447,7 @@ int main(int argc, char** argv)
// kf_output.x_.acc *= -1;
{
while (Measures.lidar_beg_time > imu_next.header.stamp.toSec()) // if it is needed for the new map?
while (Measures.lidar_beg_time > get_time_sec(imu_next.header.stamp)) // if it is needed for the new map?
{
imu_deque.pop_front();
if (imu_deque.empty())
@@ -604,7 +605,7 @@ int main(int argc, char** argv)
{
if(imu_en)
{
while (time_current > imu_next.header.stamp.toSec())
while (time_current > get_time_sec(imu_next.header.stamp))
{
imu_deque.pop_front();
if (imu_deque.empty()) break;
@@ -621,8 +622,8 @@ int main(int argc, char** argv)
}
if(imu_en && !imu_deque.empty())
{
bool last_imu = imu_next.header.stamp.toSec() == imu_deque.front()->header.stamp.toSec();
while (imu_next.header.stamp.toSec() < time_predict_last_const && !imu_deque.empty())
bool last_imu = get_time_sec(imu_next.header.stamp) == get_time_sec(imu_deque.front()->header.stamp);
while (get_time_sec(imu_next.header.stamp) < time_predict_last_const && !imu_deque.empty())
{
if (!last_imu)
{
@@ -638,7 +639,7 @@ int main(int argc, char** argv)
imu_next = *(imu_deque.front());
}
}
bool imu_comes = time_current > imu_next.header.stamp.toSec();
bool imu_comes = time_current > get_time_sec(imu_next.header.stamp);
while (imu_comes)
{
imu_upda_cov = true;
@@ -646,16 +647,16 @@ int main(int argc, char** argv)
acc_avr <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
/*** covariance update ***/
double dt = imu_next.header.stamp.toSec() - time_predict_last_const;
double dt = get_time_sec(imu_next.header.stamp) - time_predict_last_const;
kf_output.predict(dt, Q_output, input_in, true, false);
time_predict_last_const = imu_next.header.stamp.toSec(); // big problem
time_predict_last_const = get_time_sec(imu_next.header.stamp); // big problem
{
double dt_cov = imu_next.header.stamp.toSec() - time_update_last;
double dt_cov = get_time_sec(imu_next.header.stamp) - time_update_last;
if (dt_cov > 0.0)
{
time_update_last = imu_next.header.stamp.toSec();
time_update_last = get_time_sec(imu_next.header.stamp);
double propag_imu_start = omp_get_wtime();
kf_output.predict(dt_cov, Q_output, input_in, false, true);
@@ -670,7 +671,7 @@ int main(int argc, char** argv)
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
imu_comes = time_current > imu_next.header.stamp.toSec();
imu_comes = time_current > get_time_sec(imu_next.header.stamp);
}
}
if (flg_reset)
@@ -696,7 +697,7 @@ int main(int argc, char** argv)
if (feats_down_size < 1)
{
ROS_WARN("No point, skip this scan!\n");
RCLCPP_WARN(logger, "No point, skip this scan!\n");
idx += time_seq[k];
continue;
}
@@ -711,7 +712,7 @@ int main(int argc, char** argv)
{
/******* Publish odometry *******/
publish_odometry(pubOdomAftMapped);
publish_odometry(pubOdomAftMapped, tf_broadcaster);
if (runtime_pos_log)
{
euler_cur = SO3ToEuler(kf_output.x_.rot);
@@ -741,67 +742,67 @@ int main(int argc, char** argv)
imu_last = imu_next;
imu_next = *(imu_deque.front());
while (imu_next.header.stamp.toSec() > time_current && ((imu_next.header.stamp.toSec() < Measures.lidar_beg_time + lidar_time_inte )))
{ // >= ?
if (is_first_frame)
{
while (get_time_sec(imu_next.header.stamp) > time_current && ((get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)))
{ // >= ?
if (is_first_frame)
{
{
while (imu_next.header.stamp.toSec() < Measures.lidar_beg_time + lidar_time_inte)
{
// meas.imu.emplace_back(imu_deque.front()); should add to initialization
imu_deque.pop_front();
if(imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
while (get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)
{
// meas.imu.emplace_back(imu_deque.front()); should add to initialization
imu_deque.pop_front();
if(imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
}
break;
}
break;
}
angvel_avr<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
acc_avr <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
angvel_avr<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
acc_avr <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
imu_upda_cov = true;
time_update_last = time_current;
time_predict_last_const = time_current;
is_first_frame = false;
}
time_current = imu_next.header.stamp.toSec();
if (!is_first_frame)
{
double dt = time_current - time_predict_last_const;
{
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
kf_output.predict(dt_cov, Q_output, input_in, false, true);
imu_upda_cov = true;
time_update_last = time_current;
time_predict_last_const = time_current;
is_first_frame = false;
}
time_current = get_time_sec(imu_next.header.stamp);
if (!is_first_frame)
{
double dt = time_current - time_predict_last_const;
{
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
kf_output.predict(dt_cov, Q_output, input_in, false, true);
time_update_last = time_current;
}
kf_output.predict(dt, Q_output, input_in, true, false);
}
time_predict_last_const = time_current;
angvel_avr<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
acc_avr <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
// acc_avr_norm = acc_avr * G_m_s2 / acc_norm;
kf_output.update_iterated_dyn_share_IMU();
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
else
{
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
kf_output.predict(dt, Q_output, input_in, true, false);
}
time_predict_last_const = time_current;
angvel_avr<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
acc_avr <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
// acc_avr_norm = acc_avr * G_m_s2 / acc_norm;
kf_output.update_iterated_dyn_share_IMU();
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
else
{
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
}
}
}
}
@@ -811,187 +812,187 @@ int main(int argc, char** argv)
effct_feat_num = 0;
if (time_seq.size() > 0)
{
double pcl_beg_time = Measures.lidar_beg_time;
idx = -1;
for (k = 0; k < time_seq.size(); k++)
{
PointType &point_body = feats_down_body->points[idx+time_seq[k]];
time_current = point_body.curvature / 1000.0 + pcl_beg_time;
if (is_first_frame)
double pcl_beg_time = Measures.lidar_beg_time;
idx = -1;
for (k = 0; k < time_seq.size(); k++)
{
while (time_current > imu_next.header.stamp.toSec())
PointType &point_body = feats_down_body->points[idx+time_seq[k]];
time_current = point_body.curvature / 1000.0 + pcl_beg_time;
if (is_first_frame)
{
while (time_current > get_time_sec(imu_next.header.stamp))
{
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
imu_prop_cov = true;
is_first_frame = false;
t_last = time_current;
time_update_last = time_current;
{
input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
input_in.acc<<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
}
}
while (time_current > get_time_sec(imu_next.header.stamp)) // && !imu_deque.empty())
{
imu_deque.pop_front();
input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
input_in.acc <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
double dt = get_time_sec(imu_last.header.stamp) - t_last;
double dt_cov = get_time_sec(imu_last.header.stamp) - time_update_last;
if (dt_cov > 0.0)
{
kf_input.predict(dt_cov, Q_input, input_in, false, true);
time_update_last = get_time_sec(imu_last.header.stamp); //time_current;
}
kf_input.predict(dt, Q_input, input_in, true, false);
t_last = get_time_sec(imu_last.header.stamp);
imu_prop_cov = true;
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
imu_prop_cov = true;
is_first_frame = false;
// imu_upda_cov = true;
}
if (flg_reset)
{
break;
}
double dt = time_current - t_last;
t_last = time_current;
time_update_last = time_current;
{
input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
input_in.acc<<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
}
}
while (time_current > imu_next.header.stamp.toSec()) // && !imu_deque.empty())
{
imu_deque.pop_front();
double propag_start = omp_get_wtime();
input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
input_in.acc <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
double dt = imu_last.header.stamp.toSec() - t_last;
double dt_cov = imu_last.header.stamp.toSec() - time_update_last;
if (dt_cov > 0.0)
{
kf_input.predict(dt_cov, Q_input, input_in, false, true);
time_update_last = imu_last.header.stamp.toSec(); //time_current;
if(!prop_at_freq_of_imu)
{
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
kf_input.predict(dt_cov, Q_input, input_in, false, true);
time_update_last = time_current;
}
}
kf_input.predict(dt, Q_input, input_in, true, false);
t_last = imu_last.header.stamp.toSec();
imu_prop_cov = true;
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
// imu_upda_cov = true;
}
if (flg_reset)
{
break;
}
double dt = time_current - t_last;
t_last = time_current;
double propag_start = omp_get_wtime();
if(!prop_at_freq_of_imu)
{
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
kf_input.predict(dt_cov, Q_input, input_in, false, true);
time_update_last = time_current;
}
}
kf_input.predict(dt, Q_input, input_in, true, false);
propag_time += omp_get_wtime() - propag_start;
propag_time += omp_get_wtime() - propag_start;
double t_update_start = omp_get_wtime();
if (feats_down_size < 1)
{
ROS_WARN("No point, skip this scan!\n");
idx += time_seq[k];
continue;
}
if (!kf_input.update_iterated_dyn_share_modified())
{
idx = idx+time_seq[k];
continue;
}
solve_start = omp_get_wtime();
if (publish_odometry_without_downsample)
{
/******* Publish odometry *******/
publish_odometry(pubOdomAftMapped);
if (runtime_pos_log)
double t_update_start = omp_get_wtime();
if (feats_down_size < 1)
{
euler_cur = SO3ToEuler(kf_input.x_.rot);
fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << kf_input.x_.pos.transpose() << " " << kf_input.x_.vel.transpose() \
<<" "<<kf_input.x_.bg.transpose()<<" "<<kf_input.x_.ba.transpose()<<" "<<kf_input.x_.gravity.transpose()<<" "<<feats_undistort->points.size()<<endl;
}
}
RCLCPP_WARN(logger, "No point, skip this scan!\n");
for (int j = 0; j < time_seq[k]; j++)
{
PointType &point_body_j = feats_down_body->points[idx+j+1];
PointType &point_world_j = feats_down_world->points[idx+j+1];
pointBodyToWorld(&point_body_j, &point_world_j);
}
solve_time += omp_get_wtime() - solve_start;
update_time += omp_get_wtime() - t_update_start;
idx = idx + time_seq[k];
}
idx += time_seq[k];
continue;
}
if (!kf_input.update_iterated_dyn_share_modified())
{
idx = idx+time_seq[k];
continue;
}
solve_start = omp_get_wtime();
if (publish_odometry_without_downsample)
{
/******* Publish odometry *******/
publish_odometry(pubOdomAftMapped, tf_broadcaster);
if (runtime_pos_log)
{
euler_cur = SO3ToEuler(kf_input.x_.rot);
fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << kf_input.x_.pos.transpose() << " " << kf_input.x_.vel.transpose() \
<<" "<<kf_input.x_.bg.transpose()<<" "<<kf_input.x_.ba.transpose()<<" "<<kf_input.x_.gravity.transpose()<<" "<<feats_undistort->points.size()<<endl;
}
}
for (int j = 0; j < time_seq[k]; j++)
{
PointType &point_body_j = feats_down_body->points[idx+j+1];
PointType &point_world_j = feats_down_world->points[idx+j+1];
pointBodyToWorld(&point_body_j, &point_world_j);
}
solve_time += omp_get_wtime() - solve_start;
update_time += omp_get_wtime() - t_update_start;
idx = idx + time_seq[k];
}
}
else
{
if (!imu_deque.empty())
{
imu_last = imu_next;
imu_next = *(imu_deque.front());
while (imu_next.header.stamp.toSec() > time_current && ((imu_next.header.stamp.toSec() < Measures.lidar_beg_time + lidar_time_inte)))
{ // >= ?
if (is_first_frame)
{
{
{
while (imu_next.header.stamp.toSec() < Measures.lidar_beg_time + lidar_time_inte)
{
imu_deque.pop_front();
if(imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
}
break;
}
imu_prop_cov = true;
t_last = time_current;
time_update_last = time_current;
input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
input_in.acc <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
is_first_frame = false;
}
time_current = imu_next.header.stamp.toSec();
if (!is_first_frame)
{
double dt = time_current - t_last;
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
// kf_input.predict(dt_cov, Q_input, input_in, false, true);
time_update_last = imu_next.header.stamp.toSec(); //time_current;
}
// kf_input.predict(dt, Q_input, input_in, true, false);
t_last = imu_next.header.stamp.toSec();
input_in.gyro<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
input_in.acc<<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
else
{
while (get_time_sec(imu_next.header.stamp) > time_current && ((get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)))
{ // >= ?
if (is_first_frame)
{
{
{
while (get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)
{
imu_deque.pop_front();
if(imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
}
break;
}
imu_prop_cov = true;
t_last = time_current;
time_update_last = time_current;
input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
input_in.acc <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
is_first_frame = false;
}
time_current = get_time_sec(imu_next.header.stamp);
if (!is_first_frame)
{
double dt = time_current - t_last;
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
// kf_input.predict(dt_cov, Q_input, input_in, false, true);
time_update_last = get_time_sec(imu_next.header.stamp); //time_current;
}
// kf_input.predict(dt, Q_input, input_in, true, false);
t_last = get_time_sec(imu_next.header.stamp);
input_in.gyro<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
input_in.acc<<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
else
{
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
}
}
}
}
}
}
// M3D rot_cur_lidar;
@@ -1004,7 +1005,7 @@ int main(int argc, char** argv)
/******* Publish odometry downsample *******/
if (!publish_odometry_without_downsample)
{
publish_odometry(pubOdomAftMapped);
publish_odometry(pubOdomAftMapped, tf_broadcaster);
}
/*** add the feature points to map ***/

42
src/li_initialization.cpp
View File

@@ -8,8 +8,8 @@ double timediff_imu_wrt_lidar = 0.0;
bool timediff_set_flg = false;
V3D gravity_lio = V3D::Zero();
mutex mtx_buffer;
sensor_msgs::Imu imu_last, imu_next;
// sensor_msgs::Imu::ConstPtr imu_last_ptr;
sensor_msgs::msg::Imu imu_last, imu_next;
// sensor_msgs::msg::Imu::ConstSharedPtr imu_last_ptr;
PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI());
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN];
@@ -20,16 +20,17 @@ std::mutex m_time;
bool lidar_pushed = false, imu_pushed = false;
std::deque<PointCloudXYZI::Ptr> lidar_buffer;
std::deque<double> time_buffer;
std::deque<sensor_msgs::Imu::Ptr> imu_deque;
std::deque<sensor_msgs::msg::Imu::SharedPtr> imu_deque;
void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
{
// mtx_buffer.lock();
scan_count ++;
double preprocess_start_time = omp_get_wtime();
if (msg->header.stamp.toSec() < last_timestamp_lidar)
double msg_time = get_time_sec(msg->header.stamp);
if (msg_time < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
RCLCPP_ERROR(logger, "lidar loop back, clear buffer");
// lidar_buffer.shrink_to_fit();
// mtx_buffer.unlock();
@@ -37,7 +38,7 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
return;
}
last_timestamp_lidar = msg->header.stamp.toSec();
last_timestamp_lidar = msg_time;
// printf("check lidar time %f\n", last_timestamp_lidar);
// if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_deque.empty()) {
// timediff_set_flg = true;
@@ -64,7 +65,7 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
if (frame_ct == 0)
{
time_con = last_timestamp_lidar; //msg->header.stamp.toSec();
time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp);
}
if (frame_ct < 10)
{
@@ -92,7 +93,7 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
if (ptr->points.size() > 0)
{
lidar_buffer.emplace_back(ptr);
time_buffer.emplace_back(msg->header.stamp.toSec());
time_buffer.emplace_back(msg_time);
}
}
}
@@ -101,14 +102,15 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
// sig_buffer.notify_all();
}
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg)
{
// mtx_buffer.lock();
double preprocess_start_time = omp_get_wtime();
scan_count ++;
if (msg->header.stamp.toSec() < last_timestamp_lidar)
double msg_time = get_time_sec(msg->header.stamp);
if (msg_time < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
RCLCPP_ERROR(logger, "lidar loop back, clear buffer");
// mtx_buffer.unlock();
// sig_buffer.notify_all();
@@ -116,7 +118,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
// lidar_buffer.shrink_to_fit();
}
last_timestamp_lidar = msg->header.stamp.toSec();
last_timestamp_lidar = msg_time;
// if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_deque.empty()) {
// timediff_set_flg = true;
// timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar;
@@ -143,7 +145,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
if (frame_ct == 0)
{
time_con = last_timestamp_lidar; //msg->header.stamp.toSec();
time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp);
}
if (frame_ct < 10)
{
@@ -171,7 +173,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
if (ptr->points.size() > 0)
{
lidar_buffer.emplace_back(ptr);
time_buffer.emplace_back(msg->header.stamp.toSec());
time_buffer.emplace_back(msg_time);
}
}
}
@@ -180,21 +182,21 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
// sig_buffer.notify_all();
}
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in)
{
// mtx_buffer.lock();
// publish_count ++;
sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in));
msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar);
msg->header.stamp = get_ros_time(timestamp - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar);
double timestamp = msg->header.stamp.toSec();
double timestamp = get_time_sec(msg->header.stamp);
// printf("time_diff%f, %f, %f\n", last_timestamp_imu - timestamp, last_timestamp_imu, timestamp);
if (timestamp < last_timestamp_imu)
{
ROS_ERROR("imu loop back, clear deque");
RCLCPP_ERROR(logger, "imu loop back, clear deque");
// imu_deque.shrink_to_fit();
// cout << "check time:" << timestamp << ";" << last_timestamp_imu << endl;
// printf("time_diff%f, %f, %f\n", last_timestamp_imu - timestamp, last_timestamp_imu, timestamp);

12
src/li_initialization.h
View File

@@ -17,20 +17,20 @@ extern int scan_count;
extern int frame_ct, wait_num;
extern std::deque<PointCloudXYZI::Ptr> lidar_buffer;
extern std::deque<double> time_buffer;
extern std::deque<sensor_msgs::Imu::Ptr> imu_deque;
extern std::deque<sensor_msgs::msg::Imu::SharedPtr> imu_deque;
extern std::mutex m_time;
extern bool lidar_pushed, imu_pushed;
extern double imu_first_time;
extern bool lose_lid;
extern sensor_msgs::Imu imu_last, imu_next;
extern sensor_msgs::msg::Imu imu_last, imu_next;
extern PointCloudXYZI::Ptr ptr_con;
extern double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN];
// extern sensor_msgs::Imu::ConstPtr imu_last_ptr;
// extern sensor_msgs::msg::Imu::ConstSharedPtr imu_last_ptr;
void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg);
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in);
void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg);
void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in);
bool sync_packages(MeasureGroup &meas);
// #endif

172
src/parameters.cpp
View File

@@ -44,68 +44,128 @@ MeasureGroup Measures;
ofstream fout_out, fout_imu_pbp;
void readParameters(ros::NodeHandle &nh)
void readParameters(rclcpp::Node::SharedPtr &nh)
{
p_pre.reset(new Preprocess());
p_imu.reset(new ImuProcess());
nh.param<bool>("prop_at_freq_of_imu", prop_at_freq_of_imu, 1);
nh.param<bool>("use_imu_as_input", use_imu_as_input, 0);
nh.param<bool>("check_satu", check_satu, 1);
nh.param<int>("init_map_size", init_map_size, 100);
nh.param<bool>("space_down_sample", space_down_sample, 1);
nh.param<double>("mapping/satu_acc",satu_acc,3.0);
nh.param<double>("mapping/satu_gyro",satu_gyro,35.0);
nh.param<double>("mapping/acc_norm",acc_norm,1.0);
nh.param<float>("mapping/plane_thr", plane_thr, 0.05f);
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<std::string>("common/lid_topic",lid_topic,"/livox/lidar");
nh.param<std::string>("common/imu_topic", imu_topic,"/livox/imu");
nh.param<bool>("common/con_frame",con_frame,false);
nh.param<int>("common/con_frame_num",con_frame_num,1);
nh.param<bool>("common/cut_frame",cut_frame,false);
nh.param<double>("common/cut_frame_time_interval",cut_frame_time_interval,0.1);
nh.param<double>("common/time_diff_lidar_to_imu",time_diff_lidar_to_imu,0.0);
nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);
nh.param<double>("filter_size_map",filter_size_map_min,0.5);
nh->declare_parameter<bool>("prop_at_freq_of_imu", true);
nh->declare_parameter<bool>("use_imu_as_input", true);
nh->declare_parameter<bool>("check_satu", true);
nh->declare_parameter<int>("init_map_size", 100);
nh->declare_parameter<bool>("space_down_sample", true);
nh->declare_parameter<double>("mapping.satu_acc",3.0);
nh->declare_parameter<double>("mapping.satu_gyro",35.0);
nh->declare_parameter<double>("mapping.acc_norm",1.0);
nh->declare_parameter<float>("mapping.plane_thr", 0.05f);
nh->declare_parameter<int>("point_filter_num", 2);
nh->declare_parameter<std::string>("common.lid_topic","/livox/lidar");
nh->declare_parameter<std::string>("common.imu_topic","/livox/imu");
nh->declare_parameter<bool>("common.con_frame",false);
nh->declare_parameter<int>("common.con_frame_num",1);
nh->declare_parameter<bool>("common.cut_frame",false);
nh->declare_parameter<double>("common.cut_frame_time_interval",0.1);
nh->declare_parameter<double>("common.time_diff_lidar_to_imu",0.0);
nh->declare_parameter<double>("filter_size_surf",0.5);
nh->declare_parameter<double>("filter_size_map",0.5);
// nh.param<double>("cube_side_length",cube_len,2000);
nh.param<float>("mapping/det_range",DET_RANGE,300.f);
nh.param<double>("mapping/fov_degree",fov_deg,180);
nh.param<bool>("mapping/imu_en",imu_en,true);
nh.param<bool>("mapping/extrinsic_est_en",extrinsic_est_en,true);
nh.param<double>("mapping/imu_time_inte",imu_time_inte,0.005);
nh.param<double>("mapping/lidar_meas_cov",laser_point_cov,0.1);
nh.param<double>("mapping/acc_cov_input",acc_cov_input,0.1);
nh.param<double>("mapping/vel_cov",vel_cov,20);
nh.param<double>("mapping/gyr_cov_input",gyr_cov_input,0.1);
nh.param<double>("mapping/gyr_cov_output",gyr_cov_output,0.1);
nh.param<double>("mapping/acc_cov_output",acc_cov_output,0.1);
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>("mapping/imu_meas_acc_cov",imu_meas_acc_cov,0.1);
nh.param<double>("mapping/imu_meas_omg_cov",imu_meas_omg_cov,0.1);
nh.param<double>("preprocess/blind", p_pre->blind, 1.0);
nh.param<int>("preprocess/lidar_type", lidar_type, 1);
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, 1);
nh.param<double>("mapping/match_s", match_s, 81);
nh.param<std::vector<double>>("mapping/gravity", gravity, std::vector<double>());
nh.param<std::vector<double>>("mapping/gravity_init", gravity_init, std::vector<double>());
nh.param<std::vector<double>>("mapping/extrinsic_T", extrinT, std::vector<double>());
nh.param<std::vector<double>>("mapping/extrinsic_R", extrinR, std::vector<double>());
nh.param<bool>("odometry/publish_odometry_without_downsample", publish_odometry_without_downsample, false);
nh.param<bool>("publish/path_en",path_en, true);
nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en,1);
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
nh->declare_parameter<float>("mapping.det_range",300.f);
nh->declare_parameter<double>("mapping.fov_degree",180);
nh->declare_parameter<bool>("mapping.imu_en",true);
nh->declare_parameter<bool>("mapping.extrinsic_est_en",true);
nh->declare_parameter<double>("mapping.imu_time_inte",0.005);
nh->declare_parameter<double>("mapping.lidar_meas_cov",0.1);
nh->declare_parameter<double>("mapping.acc_cov_input",0.1);
nh->declare_parameter<double>("mapping.vel_cov",20);
nh->declare_parameter<double>("mapping.gyr_cov_input",0.1);
nh->declare_parameter<double>("mapping.gyr_cov_output",0.1);
nh->declare_parameter<double>("mapping.acc_cov_output",0.1);
nh->declare_parameter<double>("mapping.b_gyr_cov",0.0001);
nh->declare_parameter<double>("mapping.b_acc_cov",0.0001);
nh->declare_parameter<double>("mapping.imu_meas_acc_cov",0.1);
nh->declare_parameter<double>("mapping.imu_meas_omg_cov",0.1);
nh->declare_parameter<double>("preprocess.blind", 1.0);
nh->declare_parameter<int>("preprocess.lidar_type", 1);
nh->declare_parameter<int>("preprocess.scan_line", 16);
nh->declare_parameter<int>("preprocess.scan_rate", 10);
nh->declare_parameter<int>("preprocess.timestamp_unit", 1);
nh->declare_parameter<double>("mapping.match_s", 81);
nh->declare_parameter<std::vector<double>>("mapping.gravity", std::vector<double>());
nh->declare_parameter<std::vector<double>>("mapping.gravity_init", std::vector<double>());
nh->declare_parameter<std::vector<double>>("mapping.extrinsic_T", std::vector<double>());
nh->declare_parameter<std::vector<double>>("mapping.extrinsic_R", std::vector<double>());
nh->declare_parameter<bool>("odometry.publish_odometry_without_downsample", false);
nh->declare_parameter<bool>("publish.path_en", true);
nh->declare_parameter<bool>("publish.scan_publish_en",true);
nh->declare_parameter<bool>("publish.scan_bodyframe_pub_en",true);
nh->declare_parameter<bool>("runtime_pos_log_enable", false);
nh->declare_parameter<bool>("pcd_save.pcd_save_en", false);
nh->declare_parameter<int>("pcd_save.interval", -1);
nh->declare_parameter<double>("mapping.lidar_time_inte",0.1);
nh->declare_parameter<double>("mapping.lidar_meas_cov",0.01);
nh->declare_parameter<float>("mapping.ivox_grid_resolution", 0.2);
nh->declare_parameter<int>("ivox_nearby_type", 18);
nh->get_parameter("prop_at_freq_of_imu", prop_at_freq_of_imu);
nh->get_parameter("use_imu_as_input", use_imu_as_input);
nh->get_parameter("check_satu", check_satu);
nh->get_parameter("init_map_size", init_map_size);
nh->get_parameter("space_down_sample", space_down_sample);
nh->get_parameter("mapping.satu_acc",satu_acc);
nh->get_parameter("mapping.satu_gyro",satu_gyro);
nh->get_parameter("mapping.acc_norm",acc_norm);
nh->get_parameter("mapping.plane_thr", plane_thr);
nh->get_parameter("point_filter_num", p_pre->point_filter_num);
nh->get_parameter("common.lid_topic",lid_topic);
nh->get_parameter("common.imu_topic", imu_topic);
nh->get_parameter("common.con_frame",con_frame);
nh->get_parameter("common.con_frame_num",con_frame_num);
nh->get_parameter("common.cut_frame",cut_frame);
nh->get_parameter("common.cut_frame_time_interval",cut_frame_time_interval);
nh->get_parameter("common.time_diff_lidar_to_imu",time_diff_lidar_to_imu);
nh->get_parameter("filter_size_surf",filter_size_surf_min);
nh->get_parameter("filter_size_map",filter_size_map_min);
// nh->get_parameter("cube_side_length",cube_len);
nh->get_parameter("mapping.det_range",DET_RANGE);
nh->get_parameter("mapping.fov_degree",fov_deg);
nh->get_parameter("mapping.imu_en",imu_en);
nh->get_parameter("mapping.extrinsic_est_en",extrinsic_est_en);
nh->get_parameter("mapping.imu_time_inte",imu_time_inte);
nh->get_parameter("mapping.lidar_meas_cov",laser_point_cov);
nh->get_parameter("mapping.acc_cov_input",acc_cov_input);
nh->get_parameter("mapping.vel_cov",vel_cov);
nh->get_parameter("mapping.gyr_cov_input",gyr_cov_input);
nh->get_parameter("mapping.gyr_cov_output",gyr_cov_output);
nh->get_parameter("mapping.acc_cov_output",acc_cov_output);
nh->get_parameter("mapping.b_gyr_cov",b_gyr_cov);
nh->get_parameter("mapping.b_acc_cov",b_acc_cov);
nh->get_parameter("mapping.imu_meas_acc_cov",imu_meas_acc_cov);
nh->get_parameter("mapping.imu_meas_omg_cov",imu_meas_omg_cov);
nh->get_parameter("preprocess.blind", p_pre->blind);
nh->get_parameter("preprocess.lidar_type", lidar_type);
nh->get_parameter("preprocess.scan_line", p_pre->N_SCANS);
nh->get_parameter("preprocess.scan_rate", p_pre->SCAN_RATE);
nh->get_parameter("preprocess.timestamp_unit", p_pre->time_unit);
nh->get_parameter("mapping.match_s", match_s);
nh->get_parameter("mapping.gravity", gravity);
nh->get_parameter("mapping.gravity_init", gravity_init);
nh->get_parameter("mapping.extrinsic_T", extrinT);
nh->get_parameter("mapping.extrinsic_R", extrinR);
nh->get_parameter("odometry.publish_odometry_without_downsample", publish_odometry_without_downsample);
nh->get_parameter("publish.path_en",path_en);
nh->get_parameter("publish.scan_publish_en",scan_pub_en);
nh->get_parameter("publish.scan_bodyframe_pub_en",scan_body_pub_en);
nh->get_parameter("runtime_pos_log_enable", runtime_pos_log);
nh->get_parameter("pcd_save.pcd_save_en", pcd_save_en);
nh->get_parameter("pcd_save.interval", pcd_save_interval);
nh->get_parameter("mapping.lidar_time_inte",lidar_time_inte);
nh->get_parameter("mapping.lidar_meas_cov",laser_point_cov);
nh->get_parameter("mapping.ivox_grid_resolution", ivox_options_.resolution_);
nh->get_parameter("ivox_nearby_type", ivox_nearby_type);
nh.param<double>("mapping/lidar_time_inte",lidar_time_inte,0.1);
nh.param<double>("mapping/lidar_meas_cov",laser_point_cov,0.1);
nh.param<float>("mapping/ivox_grid_resolution", ivox_options_.resolution_, 0.2);
nh.param<int>("ivox_nearby_type", ivox_nearby_type, 18);
if (ivox_nearby_type == 0) {
ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER;
} else if (ivox_nearby_type == 6) {

14
src/parameters.h
View File

@@ -1,15 +1,15 @@
// #ifndef PARAM_H
// #define PARAM_H
#pragma once
#include <ros/ros.h>
#include <rclcpp/rclcpp.hpp>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <cstring>
#include "preprocess.h"
#include "IMU_Processing.h"
#include <sensor_msgs/NavSatFix.h>
#include <livox_ros_driver/CustomMsg.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <livox_ros_driver2/msg/custom_msg.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <mutex>
#include <omp.h>
#include <math.h>
@@ -20,9 +20,9 @@
#include <ivox/ivox3d.h>
#include <Python.h>
#include <condition_variable>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/msg/imu.hpp>
#include <pcl/common/transforms.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/msg/vector3.hpp>
// #define IVOX_NODE_TYPE_PHC
@@ -76,7 +76,7 @@ extern double time_update_last, time_current, time_predict_last_const, t_last;
extern MeasureGroup Measures;
extern ofstream fout_out, fout_imu_pbp;
void readParameters(ros::NodeHandle &n);
void readParameters(rclcpp::Node::SharedPtr &nh);
void open_file();
Eigen::Matrix<double, 3, 1> SO3ToEuler(const SO3 &orient);
void reset_cov(Eigen::Matrix<double, 24, 24> & P_init);

20
src/preprocess.cpp
View File

@@ -92,7 +92,7 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
*pcl_out = pl_surf;
}
void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg,
void Preprocess::process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg,
deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar,
const int required_frame_num, int scan_count) {
int plsize = msg->point_num;
@@ -128,7 +128,7 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::Cons
}
sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);
//end time of last frame，单位ms
double last_frame_end_time = msg->header.stamp.toSec() * 1000;
double last_frame_end_time = get_time_sec(msg->header.stamp) * 1000;
uint valid_num = 0;
uint cut_num = 0;
uint valid_pcl_size = pl_surf.points.size();
@@ -158,7 +158,7 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::Cons
}
#define MAX_LINE_NUM 128
void
Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out,
Preprocess::process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out,
deque<double> &time_lidar, const int required_frame_num, int scan_count) {
pl_surf.clear();
pl_corn.clear();
@@ -265,7 +265,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.curvature = (pl_orig.points[i].timestamp - msg->header.stamp.toSec()) * 1000; //s to ms
added_pt.curvature = (pl_orig.points[i].timestamp - get_time_sec(msg->header.stamp)) * 1000; //s to ms
double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
@@ -283,7 +283,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg
sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);
//ms
double last_frame_end_time = msg->header.stamp.toSec() * 1000;
double last_frame_end_time = get_time_sec(msg->header.stamp) * 1000;
uint valid_num = 0;
uint cut_num = 0;
uint valid_pcl_size = pl_surf.points.size();
@@ -297,7 +297,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg
PointCloudXYZI pcl_cut;
for (uint i = 1; i < valid_pcl_size; i++) {
valid_num++;
pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;
pl_surf[i].curvature += get_time_sec(msg->header.stamp) * 1000 - last_frame_end_time;
pcl_cut.push_back(pl_surf[i]);
if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {
@@ -313,7 +313,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg
}
}
void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
@@ -359,7 +359,7 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
}
void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
@@ -400,7 +400,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}
void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
@@ -505,7 +505,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
}
void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
void Preprocess::hesai_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
{
pl_surf.clear();
pl_corn.clear();

22
src/preprocess.h
View File

@@ -1,7 +1,7 @@
#include <ros/ros.h>
#include <rclcpp/rclcpp.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomMsg.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <livox_ros_driver2/msg/custom_msg.hpp>
using namespace std;
@@ -107,12 +107,12 @@ class Preprocess
Preprocess();
~Preprocess();
void process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::PointCloud2::ConstPtr pointcloud;
@@ -127,10 +127,10 @@ class Preprocess
private:
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void avia_handler(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg);
void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void hesai_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);