Update.

CMakeLists.txt

@@ -1,40 +1,21 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(point_lio)
 
 SET(CMAKE_BUILD_TYPE "Debug")
 
-ADD_COMPILE_OPTIONS(-std=c++14 )
+ADD_COMPILE_OPTIONS(-std=c++17 )
-ADD_COMPILE_OPTIONS(-std=c++14 )
+ADD_COMPILE_OPTIONS(-std=c++17 )
-set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 
+set( CMAKE_CXX_FLAGS "-std=c++17 -O3" ) 
 
 add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
 
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 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")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -std=c++0x -std=c++17 -fexceptions")
 
-message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
+#add_compile_definitions(BOOST_BIND_GLOBAL_PLACEHOLDERS)
-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()
 
 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
+# Find packages
-  geometry_msgs
+find_package(ament_cmake REQUIRED)
-  nav_msgs
+find_package(rclcpp REQUIRED)
-  sensor_msgs
+find_package(rclpy REQUIRED)
-  roscpp
+find_package(geometry_msgs REQUIRED)
-  rospy
+find_package(nav_msgs REQUIRED)
-  std_msgs
+find_package(sensor_msgs REQUIRED)
-  pcl_ros
+find_package(pcl_ros REQUIRED)
-  tf
+find_package(pcl_conversions REQUIRED)
-  livox_ros_driver
+find_package(tf2_ros REQUIRED)
-  message_generation
+find_package(tf2_eigen REQUIRED)
-  eigen_conversions
+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(
+# Declare a ROS2 executable
- DEPENDENCIES
+add_executable(pointlio_mapping src/laserMapping.cpp src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp src/IMU_Processing.cpp)
- geometry_msgs
+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
 )
-
+target_link_libraries(pointlio_mapping ${PYTHON_LIBRARIES})
-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_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()

config/avia.yaml

@@ -1,54 +1,67 @@
-common:
+/**:
-    lid_topic:  "/livox/lidar" 
+    ros__parameters:
-    imu_topic:  "/livox/imu" 
+        use_imu_as_input: True  # Change to True to use IMU as input of Point-LIO
-    con_frame: false # true: if you need to combine several LiDAR frames into one
+        prop_at_freq_of_imu: True
-    con_frame_num: 1 # the number of frames combined
+        check_satu: True
-    cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+        init_map_size: 1000
-    cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+        point_filter_num: 3  # Options: 1, 3
-    time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+        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:
+        preprocess:
-    lidar_type: 1 # 4
+            lidar_type: 1 # 4
-    scan_line: 6 # 32
+            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.
+            timestamp_unit: 1           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-    blind: 0.5 
+            blind: 0.5 
 
-mapping:
+        mapping:
-    imu_en: true
+            imu_en: true
-    extrinsic_est_en: false # for aggressive motion, set this variable false
+            extrinsic_est_en: false # for aggressive motion, set this variable false
-    imu_time_inte: 0.005 # = 1 / frequency of IMU
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
-    lidar_time_inte: 0.1
+            lidar_time_inte: 0.1
-    satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
+            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
+            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
+            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
+            lidar_meas_cov: 0.01 # 0.001; 0.01
-    acc_cov_output: 500
+            acc_cov_output: 500
-    gyr_cov_output: 1000 
+            gyr_cov_output: 1000 
-    b_acc_cov: 0.0001 
+            b_acc_cov: 0.0001 
-    b_gyr_cov: 0.0001 
+            b_gyr_cov: 0.0001 
-    imu_meas_acc_cov: 0.1 #0.1 # 0.1
+            imu_meas_acc_cov: 0.1 #0.1 # 0.1
-    imu_meas_omg_cov: 0.1 #0.01 # 0.1
+            imu_meas_omg_cov: 0.1 #0.01 # 0.1
-    gyr_cov_input: 0.01 # for IMU as input model
+            gyr_cov_input: 0.01 # for IMU as input model
-    acc_cov_input: 0.1 # 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
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
-    match_s: 81
+            match_s: 81
-    ivox_grid_resolution: 2.0
+            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: [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
+            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_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360
-    extrinsic_R: [ 1, 0, 0,
+            extrinsic_R: [ 1, 0, 0,
-                   0, 1, 0,
+                        0, 1, 0,
-                   0, 0, 1 ]
+                        0, 0, 1 ]
 
-odometry: 
+        odometry: 
-    publish_odometry_without_downsample: false
+            publish_odometry_without_downsample: false
 
-publish:
+        publish:
-    path_en: true                 # false: close the path output
+            path_en: true                 # false: close the path output
-    scan_publish_en: true         # false: close all the point cloud 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
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
 
-pcd_save:
+        pcd_save:
-    pcd_save_en: false
+            pcd_save_en: false
-    interval: -1                 # how many LiDAR frames saved in each pcd file; 
+            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.
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

config/horizon.yaml

@@ -1,54 +1,67 @@
-common:
+/**:
-    lid_topic:  "/livox/lidar"
+    ros__parameters:
-    imu_topic:  "/livox/imu"
+        use_imu_as_input: True  # Change to True to use IMU as input of Point-LIO
-    con_frame: false # true: if you need to combine several LiDAR frames into one
+        prop_at_freq_of_imu: True
-    con_frame_num: 1 # the number of frames combined
+        check_satu: True
-    cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+        init_map_size: 1000
-    cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+        point_filter_num: 3  # Options: 1, 3
-    time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+        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:
+        preprocess:
-    lidar_type: 1 
+            lidar_type: 1 
-    scan_line: 6 
+            scan_line: 6 
-    timestamp_unit: 1   # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            timestamp_unit: 1   # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-    blind: 4.0 
+            blind: 4.0 
 
-mapping:
+        mapping:
-    imu_en: true
+            imu_en: true
-    extrinsic_est_en: false # for aggressive motion, set this variable false
+            extrinsic_est_en: false # for aggressive motion, set this variable false
-    imu_time_inte: 0.005 # = 1 / frequency of IMU
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
-    lidar_time_inte: 0.1
+            lidar_time_inte: 0.1
-    satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
+            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
+            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
+            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
+            lidar_meas_cov: 0.01 # 0.001
-    acc_cov_output: 500
+            acc_cov_output: 500
-    gyr_cov_output: 1000 
+            gyr_cov_output: 1000 
-    b_acc_cov: 0.0001 
+            b_acc_cov: 0.0001 
-    b_gyr_cov: 0.0001 
+            b_gyr_cov: 0.0001 
-    imu_meas_acc_cov: 0.01 #0.1 # 2
+            imu_meas_acc_cov: 0.01 #0.1 # 2
-    imu_meas_omg_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
+            gyr_cov_input: 0.01 # for IMU as input model
-    acc_cov_input: 0.1 # 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
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
-    match_s: 81
+            match_s: 81
-    ivox_grid_resolution: 2.0
+            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: [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
+            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_T: [ 0.05512, 0.02226, -0.0297 ] 
-    extrinsic_R: [ 1, 0, 0,
+            extrinsic_R: [ 1, 0, 0,
-                   0, 1, 0,
+                        0, 1, 0,
-                   0, 0, 1 ]
+                        0, 0, 1 ]
 
-odometry: 
+        odometry: 
-    publish_odometry_without_downsample: false
+            publish_odometry_without_downsample: false
 
-publish:
+        publish:
-    path_en: true                 # false: close the path output
+            path_en: true                 # false: close the path output
-    scan_publish_en: true         # false: close all the point cloud 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
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
 
-pcd_save:
+        pcd_save:
-    pcd_save_en: false
+            pcd_save_en: false
-    interval: -1                 # how many LiDAR frames saved in each pcd file; 
+            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.
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

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.0 # 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.0
+            gyr_cov_output: 1000.0 
+            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.0
+            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, 0.0,
+                        0.0, 1.0, 0.0,
+                        0.0, 0.0, 1.0   ]
+
+        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.

config/ouster64.yaml

@@ -1,54 +1,67 @@
-common:
+/**:
-    lid_topic:  "/os_cloud_node/points"
+    ros__parameters:
-    imu_topic:  "/os_cloud_node/imu"
+        use_imu_as_input: True  # Change to True to use IMU as input of Point-LIO
-    con_frame: false # true: if you need to combine several LiDAR frames into one
+        prop_at_freq_of_imu: True
-    con_frame_num: 1 # the number of frames combined
+        check_satu: True
-    cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+        init_map_size: 1000
-    cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency 
+        point_filter_num: 3  # Options: 1, 3
-    time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+        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:
+        preprocess:
-    lidar_type: 3 # 2 #velodyne                # 1 Livox Avia LiDAR
+            lidar_type: 3 # 2 #velodyne                # 1 Livox Avia LiDAR
-    scan_line: 64 # 32 #velodyne 6 avia
+            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.
+            timestamp_unit: 3           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-    blind: 4.0
+            blind: 4.0
 
-mapping:
+        mapping:
-    imu_en: true
+            imu_en: true
-    extrinsic_est_en: false # for aggressive motion, set this variable false
+            extrinsic_est_en: false # for aggressive motion, set this variable false
-    imu_time_inte: 0.01 # = 1 / frequency of IMU
+            imu_time_inte: 0.01 # = 1 / frequency of IMU
-    lidar_time_inte: 0.1
+            lidar_time_inte: 0.1
-    satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            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
+            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
+            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
+            lidar_meas_cov: 0.01 # 0.001
-    acc_cov_output: 500
+            acc_cov_output: 500
-    gyr_cov_output: 1000 
+            gyr_cov_output: 1000 
-    b_acc_cov: 0.0001 
+            b_acc_cov: 0.0001 
-    b_gyr_cov: 0.0001 
+            b_gyr_cov: 0.0001 
-    imu_meas_acc_cov: 0.1 #0.1 # 2
+            imu_meas_acc_cov: 0.1 #0.1 # 2
-    imu_meas_omg_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
+            gyr_cov_input: 0.01 # for IMU as input model
-    acc_cov_input: 0.1 # 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
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
-    match_s: 81
+            match_s: 81
-    ivox_grid_resolution: 2.0
+            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: [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
+            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_T: [0.0, 0.0, 0.0]
-    extrinsic_R: [ 1, 0, 0,
+            extrinsic_R: [ 1, 0, 0,
-                   0, 1, 0,
+                        0, 1, 0,
-                   0, 0, 1 ]
+                        0, 0, 1 ]
 
-odometry: 
+        odometry: 
-    publish_odometry_without_downsample: false
+            publish_odometry_without_downsample: false
 
-publish:
+        publish:
-    path_en: true                 # false: close the path output
+            path_en: true                 # false: close the path output
-    scan_publish_en: true         # false: close all the point cloud 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
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
 
-pcd_save:
+        pcd_save:
-    pcd_save_en: false
+            pcd_save_en: false
-    interval: -1                 # how many LiDAR frames saved in each pcd file; 
+            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.
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

config/velody16.yaml

@@ -1,60 +1,73 @@
-common:
+/**:
-    lid_topic: "/velodyne_points" 
+    ros__parameters:
-    imu_topic: "/imu/data" 
+        use_imu_as_input: True  # Change to True to use IMU as input of Point-LIO
-    con_frame: false # true: if you need to combine several LiDAR frames into one
+        prop_at_freq_of_imu: True
-    con_frame_num: 1 # the number of frames combined
+        check_satu: True
-    cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+        init_map_size: 1000
-    cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency   
+        point_filter_num: 3  # Options: 1, 3
-    time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+        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:
+        preprocess:
-    lidar_type: 2
+            lidar_type: 2
-    scan_line: 32
+            scan_line: 32
-    timestamp_unit: 2           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            timestamp_unit: 2           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-    blind: 2.0 
+            blind: 2.0 
 
-mapping:
+        mapping:
-    imu_en: true
+            imu_en: true
-    extrinsic_est_en: false # for aggressive motion, set this variable false
+            extrinsic_est_en: false # for aggressive motion, set this variable false
-    imu_time_inte: 0.01 # = 1 / frequency of IMU
+            imu_time_inte: 0.01 # = 1 / frequency of IMU
-    lidar_time_inte: 0.1
+            lidar_time_inte: 0.1
-    satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            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
+            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
+            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
+            lidar_meas_cov: 0.01 # 0.001
-    acc_cov_output: 500
+            acc_cov_output: 500
-    gyr_cov_output: 1000 
+            gyr_cov_output: 1000 
-    b_acc_cov: 0.0001 
+            b_acc_cov: 0.0001 
-    b_gyr_cov: 0.0001 
+            b_gyr_cov: 0.0001 
-    imu_meas_acc_cov: 0.1 #0.1 # 2
+            imu_meas_acc_cov: 0.1 #0.1 # 2
-    imu_meas_omg_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
+            gyr_cov_input: 0.01 # for IMU as input model
-    acc_cov_input: 0.1 # 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
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
-    match_s: 81
+            match_s: 81
-    ivox_grid_resolution: 2.0
+            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: [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
+            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_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm
-    # extrinsic_R: [ 0, 1, 0,
+            # extrinsic_R: [ 0, 1, 0,
-    #                -1, 0, 0,
+            #                -1, 0, 0,
-    #                0, 0, 1 ] # ulhk 5 6
+            #                0, 0, 1 ] # ulhk 5 6
-    # extrinsic_R: [ 0, -1, 0,
+            # extrinsic_R: [ 0, -1, 0,
-    #                 1, 0, 0,
+            #                 1, 0, 0,
-    #                 0, 0, 1 ] # utbm 1, 2
+            #                 0, 0, 1 ] # utbm 1, 2
-    extrinsic_R: [ 1, 0, 0,
+            extrinsic_R: [ 1, 0, 0,
-                   0, 1, 0,
+                        0, 1, 0,
-                   0, 0, 1 ] # ulhk 4 utbm 3
+                        0, 0, 1 ] # ulhk 4 utbm 3
 
-odometry: 
+        odometry: 
-    publish_odometry_without_downsample: false
+            publish_odometry_without_downsample: false
 
-publish:
+        publish:
-    path_en: true                 # false: close the path output
+            path_en: true                 # false: close the path output
-    scan_publish_en: true         # false: close all the point cloud 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
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
 
-pcd_save:
+        pcd_save:
-    pcd_save_en: false
+            pcd_save_en: false
-    interval: -1                 # how many LiDAR frames saved in each pcd file; 
+            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.
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp

@@ -38,8 +38,7 @@
 
 #include <vector>
 #include <cstdlib>
-
+#include <boost/bind/bind.hpp>
-#include <boost/bind.hpp>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <Eigen/Dense>
@@ -57,6 +56,7 @@
 namespace esekfom {
 
 using namespace Eigen;
+using namespace boost::placeholders;
 
 template<typename T>
 struct dyn_share_modified

include/common_lib.h

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

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

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

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

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

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

package.xml

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

rviz_cfg/loam_livox.rviz

@@ -1,356 +1,318 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
-    Help Height: 0
+    Help Height: 78
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Axes1
+        - /Status1
-        - /mapping1
+      Splitter Ratio: 0.5
-        - /mapping1/surround1
+    Tree Height: 549
-        - /mapping1/currPoints1
+  - Class: rviz_common/Selection
-        - /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
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
-      - /2D Pose Estimate1
+      - /2D Goal Pose1
-      - /2D Nav Goal1
       - /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
+    SyncSource: CloudRegistered
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
-    - Alpha: 1
+    - Class: rviz_default_plugins/TF
-      Cell Size: 1000
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: false
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 40
-      Reference Frame: <Fixed Frame>
-      Value: false
-    - Alpha: 1
-      Class: rviz/Axes
       Enabled: true
-      Length: 0.699999988079071
+      Frame Timeout: 15
-      Name: Axes
+      Frames:
-      Radius: 0.05999999865889549
+        All Enabled: true
-      Reference Frame: body
+        aft_mapped:
-      Show Trail: false
+          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
+    - Angle Tolerance: 0.10000000149011612
-      Displays:
+      Class: rviz_default_plugins/Odometry
-        - Alpha: 1
+      Covariance:
-          Autocompute Intensity Bounds: true
+        Orientation:
-          Autocompute Value Bounds:
+          Alpha: 0.5
-            Max Value: 10
+          Color: 255; 255; 127
-            Min Value: -10
+          Color Style: Unique
-            Value: true
+          Frame: Local
-          Axis: Z
+          Offset: 1
-          Channel Name: intensity
+          Scale: 1
-          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
           Value: true
-        - Alpha: 0.10000000149011612
+        Position:
-          Autocompute Intensity Bounds: true
+          Alpha: 0.30000001192092896
-          Autocompute Value Bounds:
+          Color: 204; 51; 204
-            Max Value: 15
+          Scale: 1
-            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
           Value: true
+        Value: true
       Enabled: true
+      Keep: 100
       Name: Odometry
-    - Alpha: 1
+      Position Tolerance: 0.10000000149011612
-      Class: rviz/Axes
+      Shape:
-      Enabled: true
+        Alpha: 1
-      Length: 0.699999988079071
+        Axes Length: 1
-      Name: Axes
+        Axes Radius: 0.10000000149011612
-      Radius: 0.10000000149011612
+        Color: 255; 25; 0
-      Reference Frame: <Fixed Frame>
+        Head Length: 0.30000001192092896
-      Show Trail: false
+        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
+    - Alpha: 1
-      Buffer Length: 2
+      Buffer Length: 1
-      Class: rviz/Path
+      Class: rviz_default_plugins/Path
-      Color: 25; 255; 255
+      Color: 25; 255; 0
       Enabled: true
-      Head Diameter: 0
+      Head Diameter: 0.30000001192092896
-      Head Length: 0
+      Head Length: 0.20000000298023224
       Length: 0.30000001192092896
-      Line Style: Billboards
+      Line Style: Lines
-      Line Width: 0.20000000298023224
+      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 Diameter: 0.10000000149011612
-      Shaft Length: 0.4000000059604645
+      Shaft Length: 0.10000000149011612
-      Topic: /path
+      Topic:
-      Unreliable: false
+        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 Color: 255; 255; 255
-      Max Intensity: 0
+      Max Intensity: 184
-      Min Color: 239; 41; 41
+      Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: CloudEffected
       Position Transformer: XYZ
-      Queue Size: 10
       Selectable: true
-      Size (Pixels): 4
+      Size (Pixels): 3
-      Size (m): 0.30000001192092896
+      Size (m): 0.019999999552965164
-      Style: Spheres
+      Style: Flat Squares
-      Topic: /cloud_effected
+      Topic:
-      Unreliable: false
+        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
+        Max Value: 2.036320447921753
-        Min Value: -32.08251953125
+        Min Value: -0.09378375858068466
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
-      Color: 138; 226; 52
+      Color: 255; 255; 255
-      Color Transformer: FlatColor
+      Color Transformer: AxisColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
-      Max Color: 138; 226; 52
+      Max Color: 255; 255; 255
-      Min Color: 138; 226; 52
+      Max Intensity: 255
-      Name: PointCloud2
+      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
+      Topic:
-      Unreliable: false
+        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
+      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /MarkerArray
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
   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_default_plugins/MoveCamera
-    - Class: rviz/Select
+    - Class: rviz_default_plugins/Select
-    - Class: rviz/FocusCamera
+    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz/Measure
+    - Class: rviz_default_plugins/Measure
-    - Class: rviz/SetInitialPose
+      Line color: 128; 128; 0
-      Theta std deviation: 0.2617993950843811
+    - Class: rviz_default_plugins/SetInitialPose
-      Topic: /initialpose
+      Covariance x: 0.25
-      X std deviation: 0.5
+      Covariance y: 0.25
-      Y std deviation: 0.5
+      Covariance yaw: 0.06853891909122467
-    - Class: rviz/SetGoal
+      Topic:
-      Topic: /move_base_simple/goal
+        Depth: 5
-    - Class: rviz/PublishPoint
+        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
+      Class: rviz_default_plugins/Orbit
-      Distance: 100.72154235839844
+      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
+        X: -2.2604103088378906
-        Y: -2.53304386138916
+        Y: -0.3224470913410187
-        Z: -8.43538761138916
+        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
+      Pitch: 0.7497965693473816
-      Target Frame: global
+      Target Frame: <Fixed Frame>
-      Yaw: 4.245370388031006
+      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
+  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c80000054efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000054e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000052fc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007ea0000054e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -358,7 +320,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
+    collapsed: false
-  Width: 2488
+  Width: 1453
-  X: 72
+  X: 2404
-  Y: 27
+  Y: 218

src/IMU_Processing.cpp

@@ -1,4 +1,5 @@
 #include "IMU_Processing.h"
+#include "parameters.h"
 
 const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
 
@@ -27,7 +28,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 +72,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_)
@@ -116,7 +117,7 @@ void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_u
 
         if (init_iter_num > MAX_INI_COUNT)
         {
-          ROS_INFO("IMU Initializing: %.1f %%", 100.0);
+          RCLCPP_WARN(logger, "IMU Initializing: %.1f %%", 100.0);
           imu_need_init_ = false;
           *cur_pcl_un_ = *(meas.lidar);
         }

src/IMU_Processing.h

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

src/laserMapping.cpp

@@ -1,19 +1,19 @@
 // #include <so3_math.h>
-#include <nav_msgs/Odometry.h>
+#include <nav_msgs/msg/odometry.hpp>
-#include <nav_msgs/Path.h>
+#include <nav_msgs/msg/path.hpp>
-#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/msg/marker.hpp>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
-#include <tf/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> 
-#include <tf/transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h> 
+#include <tf2_ros/transform_broadcaster.h>
 #include "li_initialization.h"
 #include <malloc.h>
 // #include <cv_bridge/cv_bridge.h>
 // #include "matplotlibcpp.h"
-// #include <ros/console.h>
 
 using namespace std;     
 
@@ -42,14 +42,16 @@ pcl::VoxelGrid<PointType> downSizeFilterMap;
 
 V3D euler_cur;
 
-nav_msgs::Path path;
+nav_msgs::msg::Path path;
-nav_msgs::Odometry odomAftMapped;
+nav_msgs::msg::Odometry odomAftMapped;
-geometry_msgs::PoseStamped msg_body_pose;
+geometry_msgs::msg::PoseStamped msg_body_pose;
+
+rclcpp::Logger logger = rclcpp::get_logger("laserMapping");
 
 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 +151,22 @@ void MapIncremental() {
     ivox_->AddPoints(points_to_add);
 }
 
-void publish_init_map(const ros::Publisher & pubLaserCloudFullRes)
+void publish_init_map(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pubLaserCloudFullRes)
 {
     int size_init_map = init_feats_world->size();
 
-    sensor_msgs::PointCloud2 laserCloudmsg;
+    sensor_msgs::msg::PointCloud2 laserCloudmsg;
-                
+
     pcl::toROSMsg(*init_feats_world, laserCloudmsg);
-        
+
-    laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
+    laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
     laserCloudmsg.header.frame_id = "camera_init";
-    pubLaserCloudFullRes.publish(laserCloudmsg);
+    pubLaserCloudFullRes->publish(laserCloudmsg);
 }
 
 PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
 PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
-void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
+void publish_frame_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pubLaserCloudFullRes)
 {
     if (scan_pub_en)
     {
@@ -180,12 +182,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 +224,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
     }
 }
 
-void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
+void publish_frame_body(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pubLaserCloudFull_body)
 {
     int size = feats_undistort->points.size();
     PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
@@ -233,11 +235,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 +270,60 @@ void set_posestamp(T & out)
     }
 }
 
-void publish_odometry(const ros::Publisher & pubOdomAftMapped)
+void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pubOdomAftMapped,  const std::shared_ptr<tf2_ros::TransformBroadcaster> &tf_br)
 {
     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;
+    pubOdomAftMapped->publish(odomAftMapped);
-    tf::Transform                   transform;
+
-    tf::Quaternion                  q;
+    geometry_msgs::msg::TransformStamped transform;
-    transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \
+    transform.header.frame_id = "camera_init";
-                                    odomAftMapped.pose.pose.position.y, \
+    transform.child_frame_id = "body";
-                                    odomAftMapped.pose.pose.position.z));
+    transform.transform.translation.x = odomAftMapped.pose.pose.position.x;
-    q.setW(odomAftMapped.pose.pose.orientation.w);
+    transform.transform.translation.y = odomAftMapped.pose.pose.position.y;
-    q.setX(odomAftMapped.pose.pose.orientation.x);
+    transform.transform.translation.z = odomAftMapped.pose.pose.position.z;
-    q.setY(odomAftMapped.pose.pose.orientation.y);
+    transform.transform.rotation.w = odomAftMapped.pose.pose.orientation.w;
-    q.setZ(odomAftMapped.pose.pose.orientation.z);
+    transform.transform.rotation.x = odomAftMapped.pose.pose.orientation.x;
-    transform.setRotation( q );
+    transform.transform.rotation.y = odomAftMapped.pose.pose.orientation.y;
-    br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body") );
+    transform.transform.rotation.z = odomAftMapped.pose.pose.orientation.z;
+    transform.header.stamp = odomAftMapped.header.stamp;
+    tf_br->sendTransform(transform);
 }
 
-void publish_path(const ros::Publisher pubPath)
+void publish_path(const rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr & pubPath)
 {
     set_posestamp(msg_body_pose.pose);
-    // msg_body_pose.header.stamp = ros::Time::now();
+    // msg_body_pose.header.stamp = nh->get_clock()->now();
-    msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
+    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");
+    rclcpp::init(argc, argv);
-    ros::NodeHandle nh("~");
+    rclcpp::Node::SharedPtr nh = std::make_shared<rclcpp::Node>("laserMapping");
-    ros::AsyncSpinner spinner(0);
-    spinner.start();
     readParameters(nh);
     cout<<"lidar_type: "<<lidar_type<<endl;
     ivox_ = std::make_shared<IVoxType>(ivox_options_);
-    
+
-    path.header.stamp    = ros::Time().fromSec(lidar_end_time);
+    path.header.stamp    = get_ros_time(lidar_end_time);
     path.header.frame_id ="camera_init";
 
     /*** variables definition for counting ***/
@@ -371,38 +371,40 @@ int main(int argc, char** argv)
     open_file();
 
     /*** ROS subscribe initialization ***/
-    ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
+    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pcl_pc_;
-        nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
+    rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_pcl_livox_;
-        nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
+    if (p_pre->lidar_type == AVIA) {
-    ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
+        sub_pcl_livox_ = nh->create_subscription<livox_ros_driver2::msg::CustomMsg>(lid_topic, 200000, livox_pcl_cbk);
+    } else {
+        sub_pcl_pc_ = nh->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, 200000, standard_pcl_cbk);
+    }
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu = nh->create_subscription<sensor_msgs::msg::Imu>(imu_topic, 200000, imu_cbk);
 
-    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered", 1000);
-            ("/cloud_registered", 1000);
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes_body = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered_body", 1000);
-    ros::Publisher pubLaserCloudFullRes_body = nh.advertise<sensor_msgs::PointCloud2>
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_effected", 1000);
-            ("/cloud_registered_body", 1000);
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 1000);
-    // ros::Publisher pubLaserCloudEffect  = nh.advertise<sensor_msgs::PointCloud2>
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped = nh->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 1000);
-            // ("/cloud_effected", 1000);
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath = nh->create_publisher<nav_msgs::msg::Path>("/path", 1000);
-    ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub = nh->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1000);
-            ("/Laser_map", 1000);
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(nh);
-    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> 
-            ("/aft_mapped_to_init", 1000);
-    ros::Publisher pubPath          = nh.advertise<nav_msgs::Path> 
-            ("/path", 1000);
-    // ros::Publisher plane_pub = nh.advertise<visualization_msgs::Marker>
-            // ("/planner_normal", 1000);
 //------------------------------------------------------------------------------------------------------
     signal(SIGINT, SigHandle);
-    ros::Rate loop_rate(500);
+    rclcpp::Rate rate(500);
-    bool status = ros::ok();
+
-    while (status)
+    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 +435,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 +448,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 +606,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 +623,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();
+                        bool last_imu = get_time_sec(imu_next.header.stamp) == get_time_sec(imu_deque.front()->header.stamp);
-                        while (imu_next.header.stamp.toSec() < time_predict_last_const && !imu_deque.empty())
+                        while (get_time_sec(imu_next.header.stamp) < time_predict_last_const && !imu_deque.empty())
                         {
                             if (!last_imu)
                             {
@@ -638,7 +640,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 +648,16 @@ int main(int argc, char** argv)
                             acc_avr   <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
 
                             /*** covariance update ***/
-                            double dt = imu_next.header.stamp.toSec() - time_predict_last_const;
+                            double dt = get_time_sec(imu_next.header.stamp) - time_predict_last_const;
                             kf_output.predict(dt, Q_output, input_in, true, false);
-                            time_predict_last_const = imu_next.header.stamp.toSec(); // big problem
+                            time_predict_last_const = get_time_sec(imu_next.header.stamp); // big problem
-                            
+
                             {
-                                double dt_cov = imu_next.header.stamp.toSec() - time_update_last; 
+                                double dt_cov = get_time_sec(imu_next.header.stamp) - time_update_last;
 
                                 if (dt_cov > 0.0)
                                 {
-                                    time_update_last = imu_next.header.stamp.toSec();
+                                    time_update_last = get_time_sec(imu_next.header.stamp);
                                     double propag_imu_start = omp_get_wtime();
 
                                     kf_output.predict(dt_cov, Q_output, input_in, false, true);
@@ -670,7 +672,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 +698,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 +713,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 +743,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 )))
+                        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)
+                            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
+                                        while (get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)
-                                        imu_deque.pop_front();
+                                        {
-                                        if(imu_deque.empty()) break;
+                                            // meas.imu.emplace_back(imu_deque.front()); should add to initialization
-                                        imu_last = imu_next;
+                                            imu_deque.pop_front();
-                                        imu_next = *(imu_deque.front());
+                                            if(imu_deque.empty()) break;
+                                            imu_last = imu_next;
+                                            imu_next = *(imu_deque.front());
+                                        }
                                     }
+                                    break;
                                 }
-                                break;
+                                angvel_avr<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
-                            }
+                                                
-                            angvel_avr<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
+                                acc_avr   <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
-                                            
-                            acc_avr   <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
 
-                            imu_upda_cov = true;
+                                imu_upda_cov = true;
-                            time_update_last = time_current;
-                            time_predict_last_const = time_current;
-
-                                is_first_frame = false;
-                        }
-                        time_current = imu_next.header.stamp.toSec();
-
-                        if (!is_first_frame)
-                        {
-                        double dt = time_current - time_predict_last_const;
-                        {
-                            double dt_cov = time_current - time_update_last;
-                            if (dt_cov > 0.0)
-                            {
-                                kf_output.predict(dt_cov, Q_output, input_in, false, true);
                                 time_update_last = time_current;
+                                time_predict_last_const = time_current;
+
+                                    is_first_frame = false;
+                            }
+                            time_current = get_time_sec(imu_next.header.stamp);
+
+                            if (!is_first_frame)
+                            {
+                                double dt = time_current - time_predict_last_const;
+                                {
+                                    double dt_cov = time_current - time_update_last;
+                                    if (dt_cov > 0.0)
+                                    {
+                                        kf_output.predict(dt_cov, Q_output, input_in, false, true);
+                                        time_update_last = time_current;
+                                    }
+                                    kf_output.predict(dt, Q_output, input_in, true, false);
+                                }
+
+                                time_predict_last_const = time_current;
+
+                                angvel_avr<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
+                                acc_avr   <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z; 
+                                // acc_avr_norm = acc_avr * G_m_s2 / acc_norm;
+                                kf_output.update_iterated_dyn_share_IMU();
+                                imu_deque.pop_front();
+                                if (imu_deque.empty()) break;
+                                imu_last = imu_next;
+                                imu_next = *(imu_deque.front());
+                            }
+                            else
+                            {
+                                imu_deque.pop_front();
+                                if (imu_deque.empty()) break;
+                                imu_last = imu_next;
+                                imu_next = *(imu_deque.front());
                             }
-                            kf_output.predict(dt, Q_output, input_in, true, false);
                         }
-
-                        time_predict_last_const = time_current;
-
-                        angvel_avr<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
-                        acc_avr   <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z; 
-                        // acc_avr_norm = acc_avr * G_m_s2 / acc_norm;
-                        kf_output.update_iterated_dyn_share_IMU();
-                        imu_deque.pop_front();
-                        if (imu_deque.empty()) break;
-                        imu_last = imu_next;
-                        imu_next = *(imu_deque.front());
-                    }
-                    else
-                    {
-                        imu_deque.pop_front();
-                        if (imu_deque.empty()) break;
-                        imu_last = imu_next;
-                        imu_next = *(imu_deque.front());
-                    }
-                    }
                     }
                 }
             }
@@ -811,187 +813,187 @@ int main(int argc, char** argv)
                 effct_feat_num = 0;
                 if (time_seq.size() > 0)
                 {
-                double pcl_beg_time = Measures.lidar_beg_time;
+                    double pcl_beg_time = Measures.lidar_beg_time;
-                idx = -1;
+                    idx = -1;
-                for (k = 0; k < time_seq.size(); k++)
+                    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)
                     {
-                        while (time_current > imu_next.header.stamp.toSec()) 
+                        PointType &point_body  = feats_down_body->points[idx+time_seq[k]];
+                        time_current = point_body.curvature / 1000.0 + pcl_beg_time;
+                        if (is_first_frame)
+                        {
+                            while (time_current > get_time_sec(imu_next.header.stamp)) 
+                            {
+                                imu_deque.pop_front();
+                                if (imu_deque.empty()) break;
+                                imu_last = imu_next;
+                                imu_next = *(imu_deque.front());
+                            }
+                            imu_prop_cov = true;
+
+                            is_first_frame = false;
+                            t_last = time_current;
+                            time_update_last = time_current; 
+                            {
+                                input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;                 
+                                input_in.acc<<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
+                                input_in.acc = input_in.acc * G_m_s2 / acc_norm;
+                            }
+                        }
+                        
+                        while (time_current > get_time_sec(imu_next.header.stamp)) // && !imu_deque.empty())
                         {
                             imu_deque.pop_front();
+                            
+                            input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
+                            input_in.acc <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z; 
+                            input_in.acc    = input_in.acc * G_m_s2 / acc_norm; 
+                            double dt = get_time_sec(imu_last.header.stamp) - t_last;
+
+                            double dt_cov = get_time_sec(imu_last.header.stamp) - time_update_last;
+                            if (dt_cov > 0.0)
+                            {
+                                kf_input.predict(dt_cov, Q_input, input_in, false, true); 
+                                time_update_last = get_time_sec(imu_last.header.stamp); //time_current;
+                            }
+                            kf_input.predict(dt, Q_input, input_in, true, false); 
+                            t_last = get_time_sec(imu_last.header.stamp);
+                            imu_prop_cov = true;
+
                             if (imu_deque.empty()) break;
                             imu_last = imu_next;
                             imu_next = *(imu_deque.front());
-                        }
+                            // imu_upda_cov = true;
-                        imu_prop_cov = true;
+                        }     
-
+                        if (flg_reset)
-                        is_first_frame = false;
+                        {
+                            break;
+                        }     
+                        double dt = time_current - t_last;
                         t_last = time_current;
-                        time_update_last = time_current; 
+                        double propag_start = omp_get_wtime();
-                        {
-                            input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;                 
-                            input_in.acc<<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
-                            input_in.acc = input_in.acc * G_m_s2 / acc_norm;
-                        }
-                    }
-                    
-                    while (time_current > imu_next.header.stamp.toSec()) // && !imu_deque.empty())
-                    {
-                        imu_deque.pop_front();
                         
-                        input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
+                        if(!prop_at_freq_of_imu)
-                        input_in.acc <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z; 
+                        {   
-                        input_in.acc    = input_in.acc * G_m_s2 / acc_norm; 
+                            double dt_cov = time_current - time_update_last;
-                        double dt = imu_last.header.stamp.toSec() - t_last;
+                            if (dt_cov > 0.0)
-
+                            {    
-                        double dt_cov = imu_last.header.stamp.toSec() - time_update_last;
+                                kf_input.predict(dt_cov, Q_input, input_in, false, true); 
-                        if (dt_cov > 0.0)
+                                time_update_last = time_current; 
-                        {
+                            }
-                            kf_input.predict(dt_cov, Q_input, input_in, false, true); 
-                            time_update_last = imu_last.header.stamp.toSec(); //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;
+                        propag_time += omp_get_wtime() - propag_start;
-                        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;
+                        double t_update_start = omp_get_wtime();
-
+                        
-                    double t_update_start = omp_get_wtime();
+                        if (feats_down_size < 1)
-                    
-                    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)
                         {
-                            euler_cur = SO3ToEuler(kf_input.x_.rot);
+                            RCLCPP_WARN(logger, "No point, skip this scan!\n");
-                            fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << kf_input.x_.pos.transpose() << " " << kf_input.x_.vel.transpose() \
-                            <<" "<<kf_input.x_.bg.transpose()<<" "<<kf_input.x_.ba.transpose()<<" "<<kf_input.x_.gravity.transpose()<<" "<<feats_undistort->points.size()<<endl;
-                        }
-                    }
 
-                    for (int j = 0; j < time_seq[k]; j++)
+                            idx += time_seq[k];
-                    {
+                            continue;
-                        PointType &point_body_j  = feats_down_body->points[idx+j+1];
+                        }
-                        PointType &point_world_j = feats_down_world->points[idx+j+1];
+                        if (!kf_input.update_iterated_dyn_share_modified()) 
-                        pointBodyToWorld(&point_body_j, &point_world_j); 
+                        {
-                    }
+                            idx = idx+time_seq[k];
-                    solve_time += omp_get_wtime() - solve_start;
+                            continue;
-                
+                        }
-                    update_time += omp_get_wtime() - t_update_start;
+
-                    idx = idx + time_seq[k];
+                        solve_start = omp_get_wtime();
-                }  
+
+                        if (publish_odometry_without_downsample)
+                        {
+                            /******* Publish odometry *******/
+
+                            publish_odometry(pubOdomAftMapped, tf_broadcaster);
+                            if (runtime_pos_log)
+                            {
+                                euler_cur = SO3ToEuler(kf_input.x_.rot);
+                                fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << kf_input.x_.pos.transpose() << " " << kf_input.x_.vel.transpose() \
+                                <<" "<<kf_input.x_.bg.transpose()<<" "<<kf_input.x_.ba.transpose()<<" "<<kf_input.x_.gravity.transpose()<<" "<<feats_undistort->points.size()<<endl;
+                            }
+                        }
+
+                        for (int j = 0; j < time_seq[k]; j++)
+                        {
+                            PointType &point_body_j  = feats_down_body->points[idx+j+1];
+                            PointType &point_world_j = feats_down_world->points[idx+j+1];
+                            pointBodyToWorld(&point_body_j, &point_world_j); 
+                        }
+                        solve_time += omp_get_wtime() - solve_start;
+                    
+                        update_time += omp_get_wtime() - t_update_start;
+                        idx = idx + time_seq[k];
+                    }  
                 }
                 else
                 {
                     if (!imu_deque.empty())
                     { 
-                    imu_last = imu_next;
-                    imu_next = *(imu_deque.front());
-                    while (imu_next.header.stamp.toSec() > time_current && ((imu_next.header.stamp.toSec() < Measures.lidar_beg_time + lidar_time_inte)))
-                    { // >= ?
-                        if (is_first_frame)
-                        {
-                            {
-                                {
-                                    while (imu_next.header.stamp.toSec() < Measures.lidar_beg_time + lidar_time_inte)
-                                    {
-                                        imu_deque.pop_front();
-                                        if(imu_deque.empty()) break;
-                                        imu_last = imu_next;
-                                        imu_next = *(imu_deque.front());
-                                    }
-                                }
-                                
-                                break;
-                            }
-                            imu_prop_cov = true;
-                            
-                            t_last = time_current;
-                            time_update_last = time_current; 
-                            input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
-                            input_in.acc   <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
-                            input_in.acc = input_in.acc * G_m_s2 / acc_norm;
-                            
-                                is_first_frame = false;
-                            
-                        }
-                        time_current = imu_next.header.stamp.toSec();
-
-                        if (!is_first_frame)
-                        {
-                        double dt = time_current - t_last;
-
-                        double dt_cov = time_current - time_update_last;
-                        if (dt_cov > 0.0)
-                        {        
-                            // kf_input.predict(dt_cov, Q_input, input_in, false, true);
-                            time_update_last = imu_next.header.stamp.toSec(); //time_current;
-                        }
-                        // kf_input.predict(dt, Q_input, input_in, true, false);
-
-                        t_last = imu_next.header.stamp.toSec();
-                    
-                        input_in.gyro<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
-                        input_in.acc<<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z; 
-                        input_in.acc = input_in.acc * G_m_s2 / acc_norm;
-                        imu_deque.pop_front();
-                        if (imu_deque.empty()) break;
                         imu_last = imu_next;
                         imu_next = *(imu_deque.front());
-                        }
+                        while (get_time_sec(imu_next.header.stamp) > time_current && ((get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)))
-                        else
+                        { // >= ?
-                        {
+                            if (is_first_frame)
+                            {
+                                {
+                                    {
+                                        while (get_time_sec(imu_next.header.stamp) < Measures.lidar_beg_time + lidar_time_inte)
+                                        {
+                                            imu_deque.pop_front();
+                                            if(imu_deque.empty()) break;
+                                            imu_last = imu_next;
+                                            imu_next = *(imu_deque.front());
+                                        }
+                                    }
+                                    
+                                    break;
+                                }
+                                imu_prop_cov = true;
+                                
+                                t_last = time_current;
+                                time_update_last = time_current; 
+                                input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
+                                input_in.acc   <<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
+                                input_in.acc = input_in.acc * G_m_s2 / acc_norm;
+                                
+                                    is_first_frame = false;
+                                
+                            }
+                            time_current = get_time_sec(imu_next.header.stamp);
+
+                            if (!is_first_frame)
+                            {
+                            double dt = time_current - t_last;
+
+                            double dt_cov = time_current - time_update_last;
+                            if (dt_cov > 0.0)
+                            {        
+                                // kf_input.predict(dt_cov, Q_input, input_in, false, true);
+                                time_update_last = get_time_sec(imu_next.header.stamp); //time_current;
+                            }
+                            // kf_input.predict(dt, Q_input, input_in, true, false);
+
+                            t_last = get_time_sec(imu_next.header.stamp);
+                        
+                            input_in.gyro<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
+                            input_in.acc<<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z; 
+                            input_in.acc = input_in.acc * G_m_s2 / acc_norm;
                             imu_deque.pop_front();
                             if (imu_deque.empty()) break;
                             imu_last = imu_next;
                             imu_next = *(imu_deque.front());
+                            }
+                            else
+                            {
+                                imu_deque.pop_front();
+                                if (imu_deque.empty()) break;
+                                imu_last = imu_next;
+                                imu_next = *(imu_deque.front());
+                            }
                         }
                     }
-                    }
                 }
             }
             // M3D rot_cur_lidar;
@@ -1004,7 +1006,7 @@ int main(int argc, char** argv)
             /******* Publish odometry downsample *******/
             if (!publish_odometry_without_downsample)
             {
-                publish_odometry(pubOdomAftMapped);
+                publish_odometry(pubOdomAftMapped, tf_broadcaster);
             }
 
             /*** add the feature points to map ***/
@@ -1054,8 +1056,7 @@ int main(int argc, char** argv)
                 dump_lio_state_to_log(fp);
             }
         }
-        status = ros::ok();
+        rate.sleep();
-        loop_rate.sleep();
     }
     //--------------------------save map-----------------------------------
     /* 1. make sure you have enough memories

src/li_initialization.cpp

@@ -8,8 +8,8 @@ double timediff_imu_wrt_lidar = 0.0;
 bool timediff_set_flg = false;
 V3D gravity_lio = V3D::Zero();
 mutex mtx_buffer;
-sensor_msgs::Imu imu_last, imu_next;
+sensor_msgs::msg::Imu imu_last, imu_next;
-// sensor_msgs::Imu::ConstPtr imu_last_ptr;
+// sensor_msgs::msg::Imu::ConstSharedPtr imu_last_ptr;
 PointCloudXYZI::Ptr  ptr_con(new PointCloudXYZI());
 double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN];
 
@@ -20,16 +20,17 @@ std::mutex m_time;
 bool lidar_pushed = false, imu_pushed = false;
 std::deque<PointCloudXYZI::Ptr>  lidar_buffer;
 std::deque<double>               time_buffer;
-std::deque<sensor_msgs::Imu::Ptr> imu_deque;
+std::deque<sensor_msgs::msg::Imu::SharedPtr> imu_deque;
 
-void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) 
+void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) 
 {
     // mtx_buffer.lock();
     scan_count ++;
     double preprocess_start_time = omp_get_wtime();
-    if (msg->header.stamp.toSec() < last_timestamp_lidar)
+    double msg_time = get_time_sec(msg->header.stamp);
+    if (msg_time < last_timestamp_lidar)
     {
-        ROS_ERROR("lidar loop back, clear buffer");
+        RCLCPP_ERROR(logger, "lidar loop back, clear buffer");
         // lidar_buffer.shrink_to_fit();
 
         // mtx_buffer.unlock();
@@ -37,7 +38,7 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
         return;
     }
 
-    last_timestamp_lidar = msg->header.stamp.toSec();
+    last_timestamp_lidar = msg_time;
     // printf("check lidar time %f\n", last_timestamp_lidar);
     // if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_deque.empty()) {
     //     timediff_set_flg = true;
@@ -64,7 +65,7 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
     {
         if (frame_ct == 0)
         {
-            time_con = last_timestamp_lidar; //msg->header.stamp.toSec();
+            time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp);
         }
         if (frame_ct < 10)
         {
@@ -92,7 +93,7 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
         if (ptr->points.size() > 0)
         {
             lidar_buffer.emplace_back(ptr);
-            time_buffer.emplace_back(msg->header.stamp.toSec());
+            time_buffer.emplace_back(msg_time);
         }
     }
     }
@@ -101,14 +102,15 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
     // sig_buffer.notify_all();
 }
 
-void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) 
+void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg) 
 {
     // mtx_buffer.lock();
     double preprocess_start_time = omp_get_wtime();
     scan_count ++;
-    if (msg->header.stamp.toSec() < last_timestamp_lidar)
+    double msg_time = get_time_sec(msg->header.stamp);
+    if (msg_time < last_timestamp_lidar)
     {
-        ROS_ERROR("lidar loop back, clear buffer");
+        RCLCPP_ERROR(logger, "lidar loop back, clear buffer");
 
         // mtx_buffer.unlock();
         // sig_buffer.notify_all();
@@ -116,7 +118,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
         // lidar_buffer.shrink_to_fit();
     }
 
-    last_timestamp_lidar = msg->header.stamp.toSec();    
+    last_timestamp_lidar = msg_time;    
     // if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_deque.empty()) {
     //     timediff_set_flg = true;
     //     timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar;
@@ -143,7 +145,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
     {
         if (frame_ct == 0)
         {
-            time_con = last_timestamp_lidar; //msg->header.stamp.toSec();
+            time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp);
         }
         if (frame_ct < 10)
         {
@@ -171,7 +173,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
         if (ptr->points.size() > 0)
         {
             lidar_buffer.emplace_back(ptr);
-            time_buffer.emplace_back(msg->header.stamp.toSec());
+            time_buffer.emplace_back(msg_time);
         }
     }
     }
@@ -180,21 +182,22 @@ 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);
+    double timestamp = get_time_sec(msg->header.stamp);
+
+    msg->header.stamp = get_ros_time(timestamp - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar);
 
-    double timestamp = msg->header.stamp.toSec();
     // 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);
@@ -307,7 +310,7 @@ bool sync_packages(MeasureGroup &meas)
         /*** push imu data, and pop from imu buffer ***/
         if (p_imu->imu_need_init_)
         {
-            double imu_time = imu_deque.front()->header.stamp.toSec();
+            double imu_time = get_time_sec(imu_deque.front()->header.stamp);
             imu_next = *(imu_deque.front());
             meas.imu.shrink_to_fit();
             while (imu_time < lidar_end_time)
@@ -316,7 +319,7 @@ bool sync_packages(MeasureGroup &meas)
                 imu_last = imu_next;
                 imu_deque.pop_front();
                 if(imu_deque.empty()) break;
-                imu_time = imu_deque.front()->header.stamp.toSec(); // can be changed
+                imu_time = get_time_sec(imu_deque.front()->header.stamp);
                 imu_next = *(imu_deque.front());
             }
         }
@@ -328,7 +331,7 @@ bool sync_packages(MeasureGroup &meas)
         /*** push imu data, and pop from imu buffer ***/
         if (p_imu->imu_need_init_)
         {
-            double imu_time = imu_deque.front()->header.stamp.toSec();
+            double imu_time = get_time_sec(imu_deque.front()->header.stamp);
             meas.imu.shrink_to_fit();
 
             imu_next = *(imu_deque.front());
@@ -338,7 +341,7 @@ bool sync_packages(MeasureGroup &meas)
                 imu_last = imu_next;
                 imu_deque.pop_front();
                 if(imu_deque.empty()) break;
-                imu_time = imu_deque.front()->header.stamp.toSec(); // can be changed
+                imu_time = get_time_sec(imu_deque.front()->header.stamp);
                 imu_next = *(imu_deque.front());
             }
         }

src/li_initialization.h

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

src/parameters.cpp

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

src/parameters.h

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

src/preprocess.cpp

@@ -1,3 +1,4 @@
+#include "common_lib.h"
 #include "preprocess.h"
 
 #define RETURN0     0x00
@@ -44,13 +45,13 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
   point_filter_num = pfilt_num;
 }
 
-void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
+void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out)
 {  
   avia_handler(msg);
   *pcl_out = pl_surf;
 }
 
-void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
+void Preprocess::process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out)
 {
   switch (time_unit)
   {
@@ -92,8 +93,8 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
   *pcl_out = pl_surf;
 }
 
-void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg,
+void Preprocess::process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg,
-                                         deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar,
+                                         std::deque<PointCloudXYZI::Ptr> &pcl_out, std::deque<double> &time_lidar,
                                          const int required_frame_num, int scan_count) {
     int plsize = msg->point_num;
     pl_surf.clear();
@@ -128,7 +129,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();
@@ -141,7 +142,7 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::Cons
     for (uint i = 1; i < valid_pcl_size; i++) {
         valid_num++;
         //Compute new opffset time of each point：ms
-        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)) {
             cut_num++;
@@ -158,8 +159,8 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::Cons
 }
 #define MAX_LINE_NUM 128
 void
-Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out,
+Preprocess::process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, std::deque<PointCloudXYZI::Ptr> &pcl_out,
-                                   deque<double> &time_lidar, const int required_frame_num, int scan_count) {
+                                   std::deque<double> &time_lidar, const int required_frame_num, int scan_count) {
     pl_surf.clear();
     pl_corn.clear();
     pl_full.clear();
@@ -265,7 +266,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 +284,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 +298,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 +314,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 +360,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();
@@ -371,7 +372,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   pl_surf.reserve(plsize);
   
   
-  double time_stamp = msg->header.stamp.toSec();
+  double time_stamp = get_time_sec(msg->header.stamp);
   // cout << "===================================" << endl;
   // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
   for (int i = 0; i < pl_orig.points.size(); i++)
@@ -400,7 +401,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 +506,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();
@@ -872,10 +873,10 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
   }
 }
 
-void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)
+void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct)
 {
   pl.height = 1; pl.width = pl.size();
-  sensor_msgs::PointCloud2 output;
+  sensor_msgs::msg::PointCloud2 output;
   pcl::toROSMsg(pl, output);
   output.header.frame_id = "livox";
   output.header.stamp = ct;

src/preprocess.h

@@ -1,7 +1,14 @@
-#include <ros/ros.h>
+#pragma once
+
+#include <common_lib.h>
+#include <rclcpp/rclcpp.hpp>
 #include <pcl_conversions/pcl_conversions.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
-#include <livox_ros_driver/CustomMsg.h>
+#include <livox_ros_driver2/msg/custom_msg.hpp>
+
+#include <cmath>
+#include <deque>
+#include <vector>
 
 using namespace std;
 
@@ -107,12 +114,12 @@ class Preprocess
   Preprocess();
   ~Preprocess();
   
-  void process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
+  void process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, std::deque<PointCloudXYZI::Ptr> &pcl_out, std::deque<double> &time_lidar, const int required_frame_num, int scan_count);
   
-  void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
+  void process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, std::deque<PointCloudXYZI::Ptr> &pcl_out, std::deque<double> &time_lidar, const int required_frame_num, int scan_count);
  
-  void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
+  void process(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
-  void process(const sensor_msgs::PointCloud2::ConstPtr &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;
@@ -123,16 +130,16 @@ class Preprocess
   int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
   double blind, det_range;
   bool given_offset_time;
-  ros::Publisher pub_full, pub_surf, pub_corn;
+  // ros::Publisher pub_full, pub_surf, pub_corn;
     
 
   private:
-  void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
+  void avia_handler(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg);
-  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
-  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
-  void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void hesai_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
   void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
-  void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
+  void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct);
   int  plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
   bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
   bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);