diff --git a/CMakeLists.txt b/CMakeLists.txt index 13ca311..005b8cd 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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() diff --git a/config/avia.yaml b/config/avia.yaml index 374bfba..2c9ecb9 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -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. \ No newline at end of file + 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. \ No newline at end of file diff --git a/config/horizon.yaml b/config/horizon.yaml index 81ebac2..88c0de9 100755 --- a/config/horizon.yaml +++ b/config/horizon.yaml @@ -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. \ No newline at end of file + 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. \ No newline at end of file diff --git a/config/mid360.yaml b/config/mid360.yaml new file mode 100644 index 0000000..4722249 --- /dev/null +++ b/config/mid360.yaml @@ -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. \ No newline at end of file diff --git a/config/ouster64.yaml b/config/ouster64.yaml index 06edadf..e31d1e9 100755 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -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. \ No newline at end of file + 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. \ No newline at end of file diff --git a/config/velody16.yaml b/config/velody16.yaml index 825c8db..9e9c827 100755 --- a/config/velody16.yaml +++ b/config/velody16.yaml @@ -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. \ No newline at end of file + 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. \ No newline at end of file diff --git a/include/common_lib.h b/include/common_lib.h index e0f218f..600b01c 100755 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -5,10 +5,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> #include @@ -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 imu; + deque imu; }; template @@ -236,4 +236,17 @@ bool esti_plane(Matrix &pca_result, const PointVector &point, const T & // template // 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 \ No newline at end of file diff --git a/launch/mapping_avia.launch.py b/launch/mapping_avia.launch.py new file mode 100644 index 0000000..dc067f7 --- /dev/null +++ b/launch/mapping_avia.launch.py @@ -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 diff --git a/launch/mapping_horizon.launch.py b/launch/mapping_horizon.launch.py new file mode 100644 index 0000000..8b3374b --- /dev/null +++ b/launch/mapping_horizon.launch.py @@ -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 diff --git a/launch/mapping_mid360.launch.py b/launch/mapping_mid360.launch.py new file mode 100644 index 0000000..5fc9c2e --- /dev/null +++ b/launch/mapping_mid360.launch.py @@ -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 diff --git a/launch/mapping_ouster64.launch.py b/launch/mapping_ouster64.launch.py new file mode 100644 index 0000000..5577693 --- /dev/null +++ b/launch/mapping_ouster64.launch.py @@ -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 diff --git a/launch/mapping_velody16.launch.py b/launch/mapping_velody16.launch.py new file mode 100644 index 0000000..21cd90b --- /dev/null +++ b/launch/mapping_velody16.launch.py @@ -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 diff --git a/package.xml b/package.xml index 15ba3fd..aff7da3 100755 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + point_lio 0.0.0 @@ -16,32 +16,25 @@ Dongjiao He - catkin - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - sensor_msgs - tf - pcl_ros - livox_ros_driver - message_generation + + ament_cmake - geometry_msgs - nav_msgs - sensor_msgs - roscpp - rospy - std_msgs - tf - pcl_ros - livox_ros_driver - message_runtime + + rclcpp + rclpy + sensor_msgs + geometry_msgs + nav_msgs + tf2_ros + pcl_ros + pcl_conversions + livox_ros_driver2 - rostest - rosbag + + ament_lint_auto + ament_lint_common + ament_cmake - + \ No newline at end of file diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index 8cf6ece..b6f0986 100755 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -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: - 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: - 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: + 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 diff --git a/src/IMU_Processing.cpp b/src/IMU_Processing.cpp index 0f51d2e..dcfef1a 100755 --- a/src/IMU_Processing.cpp +++ b/src/IMU_Processing.cpp @@ -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_) diff --git a/src/IMU_Processing.h b/src/IMU_Processing.h index a7f888e..280c774 100755 --- a/src/IMU_Processing.h +++ b/src/IMU_Processing.h @@ -5,7 +5,7 @@ // #include // #include #include -#include +#include // #include #include // #include "Estimator.h" @@ -13,12 +13,12 @@ #include #include #include -#include +#include #include -#include -#include +#include +#include #include -#include +#include /// *************Preconfiguration diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 2c360de..fc02e26 100755 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -1,19 +1,19 @@ // #include -#include -#include -#include +#include +#include +#include #include #include #include #include #include -#include -#include +#include +#include +#include #include "li_initialization.h" #include // #include // #include "matplotlibcpp.h" -// #include using namespace std; @@ -42,14 +42,14 @@ pcl::VoxelGrid 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::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::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::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::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::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("laserMapping"); readParameters(nh); cout<<"lidar_type: "<(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::SharedPtr sub_pcl_pc_; + rclcpp::Subscription::SharedPtr sub_pcl_livox_; + if (p_pre->lidar_type == AVIA) { + sub_pcl_livox_ = nh->create_subscription(lid_topic, 200000, livox_pcl_cbk); + } else { + sub_pcl_pc_ = nh->create_subscription(lid_topic, 200000, standard_pcl_cbk); + } + rclcpp::Subscription::SharedPtr sub_imu = nh->create_subscription(imu_topic, 200000, imu_cbk); - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/cloud_registered", 1000); - ros::Publisher pubLaserCloudFullRes_body = nh.advertise - ("/cloud_registered_body", 1000); - // ros::Publisher pubLaserCloudEffect = nh.advertise - // ("/cloud_effected", 1000); - ros::Publisher pubLaserCloudMap = nh.advertise - ("/Laser_map", 1000); - ros::Publisher pubOdomAftMapped = nh.advertise - ("/aft_mapped_to_init", 1000); - ros::Publisher pubPath = nh.advertise + rclcpp::Publisher::SharedPtr pubLaserCloudFullRes = nh->create_publisher("/cloud_registered", 1000); + rclcpp::Publisher::SharedPtr pubLaserCloudFullRes_body = nh->create_publisher("/cloud_registered_body", 1000); + rclcpp::Publisher::SharedPtr pubLaserCloudEffect = nh->create_publisher("/cloud_effected", 1000); + rclcpp::Publisher::SharedPtr pubLaserCloudMap = nh->create_publisher("/Laser_map", 1000); + rclcpp::Publisher::SharedPtr pubOdomAftMapped = nh->create_publisher("/aft_mapped_to_init", 1000); + rclcpp::Publisher::SharedPtr pubPath = nh->create_publisher("/path", 1000); ("/path", 1000); - // ros::Publisher plane_pub = nh.advertise - // ("/planner_normal", 1000); + rclcpp::Publisher::SharedPtr plane_pub = nh->create_publisher("/planner_normal", 1000); + tf2_ros::TransformBroadcaster::SharedPtr tf_broadcaster = std::make_shared(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 < 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< 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< 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< get_time_sec(imu_next.header.stamp)) // && !imu_deque.empty()) { imu_deque.pop_front(); + + input_in.gyro< 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_next.header.stamp.toSec()) // && !imu_deque.empty()) - { - imu_deque.pop_front(); + double propag_start = omp_get_wtime(); - input_in.gyro< 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() \ - <<" "<points.size()<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() \ + <<" "<points.size()<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< 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< 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< 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< lidar_buffer; std::deque time_buffer; -std::deque imu_deque; +std::deque 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); diff --git a/src/li_initialization.h b/src/li_initialization.h index 555773c..e8e560d 100755 --- a/src/li_initialization.h +++ b/src/li_initialization.h @@ -17,20 +17,20 @@ extern int scan_count; extern int frame_ct, wait_num; extern std::deque lidar_buffer; extern std::deque time_buffer; -extern std::deque imu_deque; +extern std::deque 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 \ No newline at end of file diff --git a/src/parameters.cpp b/src/parameters.cpp index 0c46d80..d69c59c 100755 --- a/src/parameters.cpp +++ b/src/parameters.cpp @@ -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("prop_at_freq_of_imu", prop_at_freq_of_imu, 1); - nh.param("use_imu_as_input", use_imu_as_input, 0); - nh.param("check_satu", check_satu, 1); - nh.param("init_map_size", init_map_size, 100); - nh.param("space_down_sample", space_down_sample, 1); - nh.param("mapping/satu_acc",satu_acc,3.0); - nh.param("mapping/satu_gyro",satu_gyro,35.0); - nh.param("mapping/acc_norm",acc_norm,1.0); - nh.param("mapping/plane_thr", plane_thr, 0.05f); - nh.param("point_filter_num", p_pre->point_filter_num, 2); - nh.param("common/lid_topic",lid_topic,"/livox/lidar"); - nh.param("common/imu_topic", imu_topic,"/livox/imu"); - nh.param("common/con_frame",con_frame,false); - nh.param("common/con_frame_num",con_frame_num,1); - nh.param("common/cut_frame",cut_frame,false); - nh.param("common/cut_frame_time_interval",cut_frame_time_interval,0.1); - nh.param("common/time_diff_lidar_to_imu",time_diff_lidar_to_imu,0.0); - nh.param("filter_size_surf",filter_size_surf_min,0.5); - nh.param("filter_size_map",filter_size_map_min,0.5); + nh->declare_parameter("prop_at_freq_of_imu", true); + nh->declare_parameter("use_imu_as_input", true); + nh->declare_parameter("check_satu", true); + nh->declare_parameter("init_map_size", 100); + nh->declare_parameter("space_down_sample", true); + nh->declare_parameter("mapping.satu_acc",3.0); + nh->declare_parameter("mapping.satu_gyro",35.0); + nh->declare_parameter("mapping.acc_norm",1.0); + nh->declare_parameter("mapping.plane_thr", 0.05f); + nh->declare_parameter("point_filter_num", 2); + nh->declare_parameter("common.lid_topic","/livox/lidar"); + nh->declare_parameter("common.imu_topic","/livox/imu"); + nh->declare_parameter("common.con_frame",false); + nh->declare_parameter("common.con_frame_num",1); + nh->declare_parameter("common.cut_frame",false); + nh->declare_parameter("common.cut_frame_time_interval",0.1); + nh->declare_parameter("common.time_diff_lidar_to_imu",0.0); + nh->declare_parameter("filter_size_surf",0.5); + nh->declare_parameter("filter_size_map",0.5); // nh.param("cube_side_length",cube_len,2000); - nh.param("mapping/det_range",DET_RANGE,300.f); - nh.param("mapping/fov_degree",fov_deg,180); - nh.param("mapping/imu_en",imu_en,true); - nh.param("mapping/extrinsic_est_en",extrinsic_est_en,true); - nh.param("mapping/imu_time_inte",imu_time_inte,0.005); - nh.param("mapping/lidar_meas_cov",laser_point_cov,0.1); - nh.param("mapping/acc_cov_input",acc_cov_input,0.1); - nh.param("mapping/vel_cov",vel_cov,20); - nh.param("mapping/gyr_cov_input",gyr_cov_input,0.1); - nh.param("mapping/gyr_cov_output",gyr_cov_output,0.1); - nh.param("mapping/acc_cov_output",acc_cov_output,0.1); - nh.param("mapping/b_gyr_cov",b_gyr_cov,0.0001); - nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); - nh.param("mapping/imu_meas_acc_cov",imu_meas_acc_cov,0.1); - nh.param("mapping/imu_meas_omg_cov",imu_meas_omg_cov,0.1); - nh.param("preprocess/blind", p_pre->blind, 1.0); - nh.param("preprocess/lidar_type", lidar_type, 1); - nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); - nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); - nh.param("preprocess/timestamp_unit", p_pre->time_unit, 1); - nh.param("mapping/match_s", match_s, 81); - nh.param>("mapping/gravity", gravity, std::vector()); - nh.param>("mapping/gravity_init", gravity_init, std::vector()); - nh.param>("mapping/extrinsic_T", extrinT, std::vector()); - nh.param>("mapping/extrinsic_R", extrinR, std::vector()); - nh.param("odometry/publish_odometry_without_downsample", publish_odometry_without_downsample, false); - nh.param("publish/path_en",path_en, true); - nh.param("publish/scan_publish_en",scan_pub_en,1); - nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en,1); - nh.param("runtime_pos_log_enable", runtime_pos_log, 0); - nh.param("pcd_save/pcd_save_en", pcd_save_en, false); - nh.param("pcd_save/interval", pcd_save_interval, -1); + nh->declare_parameter("mapping.det_range",300.f); + nh->declare_parameter("mapping.fov_degree",180); + nh->declare_parameter("mapping.imu_en",true); + nh->declare_parameter("mapping.extrinsic_est_en",true); + nh->declare_parameter("mapping.imu_time_inte",0.005); + nh->declare_parameter("mapping.lidar_meas_cov",0.1); + nh->declare_parameter("mapping.acc_cov_input",0.1); + nh->declare_parameter("mapping.vel_cov",20); + nh->declare_parameter("mapping.gyr_cov_input",0.1); + nh->declare_parameter("mapping.gyr_cov_output",0.1); + nh->declare_parameter("mapping.acc_cov_output",0.1); + nh->declare_parameter("mapping.b_gyr_cov",0.0001); + nh->declare_parameter("mapping.b_acc_cov",0.0001); + nh->declare_parameter("mapping.imu_meas_acc_cov",0.1); + nh->declare_parameter("mapping.imu_meas_omg_cov",0.1); + nh->declare_parameter("preprocess.blind", 1.0); + nh->declare_parameter("preprocess.lidar_type", 1); + nh->declare_parameter("preprocess.scan_line", 16); + nh->declare_parameter("preprocess.scan_rate", 10); + nh->declare_parameter("preprocess.timestamp_unit", 1); + nh->declare_parameter("mapping.match_s", 81); + nh->declare_parameter>("mapping.gravity", std::vector()); + nh->declare_parameter>("mapping.gravity_init", std::vector()); + nh->declare_parameter>("mapping.extrinsic_T", std::vector()); + nh->declare_parameter>("mapping.extrinsic_R", std::vector()); + nh->declare_parameter("odometry.publish_odometry_without_downsample", false); + nh->declare_parameter("publish.path_en", true); + nh->declare_parameter("publish.scan_publish_en",true); + nh->declare_parameter("publish.scan_bodyframe_pub_en",true); + nh->declare_parameter("runtime_pos_log_enable", false); + nh->declare_parameter("pcd_save.pcd_save_en", false); + nh->declare_parameter("pcd_save.interval", -1); + + nh->declare_parameter("mapping.lidar_time_inte",0.1); + nh->declare_parameter("mapping.lidar_meas_cov",0.01); + nh->declare_parameter("mapping.ivox_grid_resolution", 0.2); + nh->declare_parameter("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("mapping/lidar_time_inte",lidar_time_inte,0.1); - nh.param("mapping/lidar_meas_cov",laser_point_cov,0.1); - nh.param("mapping/ivox_grid_resolution", ivox_options_.resolution_, 0.2); - nh.param("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) { diff --git a/src/parameters.h b/src/parameters.h index 3b408a7..2f400a1 100755 --- a/src/parameters.h +++ b/src/parameters.h @@ -1,15 +1,15 @@ // #ifndef PARAM_H // #define PARAM_H #pragma once -#include +#include #include #include #include #include "preprocess.h" #include "IMU_Processing.h" -#include -#include -#include +#include +#include +#include #include #include #include @@ -20,9 +20,9 @@ #include #include #include -#include +#include #include -#include +#include // #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 SO3ToEuler(const SO3 &orient); void reset_cov(Eigen::Matrix & P_init); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 1850cdf..a8c9715 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -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 &pcl_out, deque &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 &pcl_out, +Preprocess::process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, deque &pcl_out, deque &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(); diff --git a/src/preprocess.h b/src/preprocess.h index 1c031fd..f6fdce3 100755 --- a/src/preprocess.h +++ b/src/preprocess.h @@ -1,7 +1,7 @@ -#include +#include #include -#include -#include +#include +#include using namespace std; @@ -107,12 +107,12 @@ class Preprocess Preprocess(); ~Preprocess(); - void process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count); + void process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count); - void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count); + void process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, deque &pcl_out, deque &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 &types); void pub_func(PointCloudXYZI &pl, const ros::Time &ct); int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);