diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7f51009e..e1644bd6 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.10)
 project(fast_livo)
 
 set(CMAKE_BUILD_TYPE "Release")
@@ -8,6 +8,10 @@ set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
+IF("$ENV{ROS_DISTRO}" STRLESS "iron")
+  add_definitions(-DPRE_ROS_IRON)
+ENDIF()
+
 # Set common compile options
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions")
 
@@ -75,57 +79,93 @@ else()
   message(STATUS "mimalloc not found, proceeding without it")
 endif()
 
-# Find catkin and required dependencies
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  roscpp
-  rospy
-  std_msgs
-  pcl_ros
-  tf
-  livox_ros_driver
-  message_generation
-  eigen_conversions
-  vikit_common
-  vikit_ros
-  cv_bridge
-  image_transport
-)
-
+# Find ament and required dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
+find_package(vikit_common REQUIRED)
+find_package(vikit_ros REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
 
-# Define the catkin package
-catkin_package(
-  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge vikit_common vikit_ros image_transport
-  DEPENDS EIGEN3 PCL OpenCV Sophus
-)
-
 # Include directories for dependencies
 include_directories(
-  ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
   ${Sophus_INCLUDE_DIRS}
+  ${vikit_common_INCLUDE_DIRS}
+  ${vikit_ros_INCLUDE_DIRS}
   include
 )
 
+set(dependencies
+  rclcpp
+  rclpy
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  visualization_msgs
+  cv_bridge
+  vikit_common
+  vikit_ros
+  image_transport
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  livox_ros_driver2
+)
+
+set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX)
+
+# link_directories(${COMMON_DEPENDENCIES}
+#   ${vikit_common_LIBRARIES}/libvikit_common.so
+#   ${vikit_ros_LIBRARIES}/libvikit_ros.so
+# )
+
 # Add libraries
 add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp)
 add_library(lio src/voxel_map.cpp)
 add_library(pre src/preprocess.cpp)
 add_library(imu_proc src/IMU_Processing.cpp)
 add_library(laser_mapping src/LIVMapper.cpp)
+add_library(utils src/utils.cpp)
+
+ament_target_dependencies(vio ${dependencies} )
+ament_target_dependencies(lio ${dependencies})
+ament_target_dependencies(pre ${dependencies})
+ament_target_dependencies(imu_proc ${dependencies})
+ament_target_dependencies(laser_mapping ${dependencies})
+
+# linking libraries or executables to public dependencies
+target_link_libraries(laser_mapping 
+    ${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so 
+    ${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so
+    ${COMMON_DEPENDENCIES}
+)
+target_link_libraries(vio ${COMMON_DEPENDENCIES})
+target_link_libraries(lio utils ${COMMON_DEPENDENCIES})
+target_link_libraries(pre ${COMMON_DEPENDENCIES})
+target_link_libraries(imu_proc ${COMMON_DEPENDENCIES})
 
 # Add the main executable
 add_executable(fastlivo_mapping src/main.cpp)
 
+ament_target_dependencies(fastlivo_mapping ${dependencies})
+
 # Link libraries to the executable
 target_link_libraries(fastlivo_mapping
   laser_mapping
@@ -133,7 +173,6 @@ target_link_libraries(fastlivo_mapping
   lio
   pre
   imu_proc
-  ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${Sophus_LIBRARIES}
@@ -143,4 +182,34 @@ target_link_libraries(fastlivo_mapping
 # Link mimalloc if found
 if(mimalloc_FOUND)
   target_link_libraries(fastlivo_mapping mimalloc)
-endif()
\ No newline at end of file
+endif()
+
+# Install the executable
+install(TARGETS
+  fastlivo_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
+  livox_ros_driver2
+  Eigen3
+  PCL
+  OpenCV 
+  Sophus
+)
+
+ament_package()
\ No newline at end of file
diff --git a/README.md b/README.md
index eac5d4e6..1081339b 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,9 @@
-# FAST-LIVO2
+# FAST-LIVO2 ROS2 HUMBLE
 
 ## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry
 
+Thanks to hku mars lab chunran zheng for the open source excellent work
+
 ### 📢 News
 
 - 🔓 **2025-01-23**: Code released!  
@@ -37,11 +39,14 @@ We open-source our handheld device, including CAD files, synchronization scheme,
 ### 1.4 Our associate dataset: FAST-LIVO2-Dataset
 Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z) used for evaluation is also available online. **Please note that the dataset is being uploaded gradually.**
 
+### MARS-LVIG dataset
+[**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)：A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion
+
 ## 2. Prerequisited
 
 ### 2.1 Ubuntu and ROS
 
-Ubuntu 16.04~20.04.  [ROS Installation](http://wiki.ros.org/ROS/Installation).
+Ubuntu 22.04.  [ROS Installation](http://wiki.ros.org/ROS/Installation).
 
 ### 2.2 PCL && Eigen && OpenCV
 
@@ -64,41 +69,111 @@ make
 sudo make install
 ```
 
+if build fails due to `so2.cpp:32:26: error: lvalue required as left operand of assignment`, modify the code as follows:
+
+**so2.cpp**
+```diff
+namespace Sophus
+{
+
+SO2::SO2()
+{
+-  unit_complex_.real() = 1.;
+-  unit_complex_.imag() = 0.;
++  unit_complex_.real(1.);
++  unit_complex_.imag(0.);
+}
+```
+
 ### 2.4 Vikit
 
 Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder.
 
+For well-known reasons, ROS2 does not have a direct global parameter server and a simple method to obtain the corresponding parameters. For details, please refer to https://discourse.ros.org/t/ros2-global-parameter-server-status/10114/11. I use a special way to get camera parameters in Vikit. While the method I've provided so far is quite simple and not perfect, it meets my needs. More contributions to improve `rpg_vikit` are hoped.
+
 ```bash
 # Different from the one used in fast-livo1
-cd catkin_ws/src
-git clone https://github.com/xuankuzcr/rpg_vikit.git 
+cd fast_ws/src
+git clone https://github.com/Robotic-Developer-Road/rpg_vikit.git 
 ```
 
-### 2.5 **livox_ros_driver**
+Thanks to the following repositories for the code reference:
+
+- [uzh-rpg/rpg_vikit](https://github.com/uzh-rpg/rpg_vikit)
+- [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit)
+- [uavfly/vikit](https://github.com/uavfly/vikit)
 
-Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).
+### 2.5 **livox_ros_driver2**
+
+Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
+
+why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient.
 
 ## 3. Build
 
-Clone the repository and catkin_make:
+Clone the repository and colcon build:
 
 ```
-cd ~/catkin_ws/src
-git clone https://github.com/hku-mars/FAST-LIVO2
+cd ~/fast_ws/src
+git clone https://github.com/Robotic-Developer-Road/FAST-LIVO2.git
 cd ../
-catkin_make
-source ~/catkin_ws/devel/setup.bash
+colcon build --symlink-install --continue-on-error
+source ~/fast_ws/install/setup.bash
 ```
 
 ## 4. Run our examples
 
 Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). 
 
+### convert rosbag
+
+convert ROS1 rosbag to ROS2 rosbag
+```bash
+pip install rosbags
+rosbags-convert --src Retail_Street.bag --dst Retail_Street
 ```
-roslaunch fast_livo mapping_avia.launch
-rosbag play YOUR_DOWNLOADED.bag
+- [gitlab rosbags](https://gitlab.com/ternaris/rosbags)
+- [pypi rosbags](https://pypi.org/project/rosbags/)
+
+### change the msg type on rosbag
+
+Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file. 
+1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2.
+2. change the msg type of msg type in **metadata.yaml** as follows:
+
+**metadata.yaml**
+```diff
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: {}
+  duration:
+    nanoseconds: 135470252209
+  files:
+  - duration:
+      nanoseconds: 135470252209
+    message_count: 30157
+    path: Retail_Street.db3
+    ..............
+    topic_metadata:
+      name: /livox/lidar
+      offered_qos_profiles: ''
+      serialization_format: cdr
+-     type: livox_ros_driver/msg/CustomMsg
++     type: livox_ros_driver2/msg/CustomMsg
+      type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d
+    ...............
+.....
 ```
 
+### Run the demo
+
+Do not forget to `source` your ROS2 workspace before running the following command.
+
+```bash
+ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True
+ros2 bag play -p Retail_Street  # space bar controls play/pause
+```
 
 ## 5. License
 
diff --git a/config/MARS_LVIG.yaml b/config/MARS_LVIG.yaml
index 34b84fc7..5c7c9e1e 100644
--- a/config/MARS_LVIG.yaml
+++ b/config/MARS_LVIG.yaml
@@ -1,111 +1,113 @@
-common:
-  img_topic: "/left_camera/image"
-  lid_topic: "/livox/lidar"
-  imu_topic: "/livox/imu"
-  img_en: 1
-  lidar_en: 1
-  ros_driver_bug_fix: false
+/**:
+  ros__parameters:
+    common:
+      img_topic: "/left_camera/image"
+      lid_topic: "/livox/lidar"
+      imu_topic: "/livox/imu"
+      img_en: 1
+      lidar_en: 1
+      ros_driver_bug_fix: false
 
-extrin_calib:
-  extrinsic_T: [0.04165, 0.02326, -0.0284]
-  extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-  # MARS_LVIG HKisland HKairport
-  Rcl: [0.00438814,-0.999807,-0.0191582,
-        -0.00978695,0.0191145,-0.999769,
-        0.999942,0.00457463,-0.00970118]
-  Pcl: [0.016069, 0.0871753, -0.0718021]
-  # MARS_LVIG AMtown AMvalley
-  # Rcl: [ -0.0022464, -0.9997299, -0.0231319,
-  #        -0.0084211,  0.0231501, -0.9996966,
-  #         0.9999620, -0.0020509, -0.0084708]
-  # Pcl: [-0.0025563, 0.0567484, -0.0512149]
+    extrin_calib:
+      extrinsic_T: [0.04165, 0.02326, -0.0284]
+      extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+      # MARS_LVIG HKisland HKairport
+      Rcl: [0.00438814,-0.999807,-0.0191582,
+            -0.00978695,0.0191145,-0.999769,
+            0.999942,0.00457463,-0.00970118]
+      Pcl: [0.016069, 0.0871753, -0.0718021]
+      # MARS_LVIG AMtown AMvalley
+      # Rcl: [ -0.0022464, -0.9997299, -0.0231319,
+      #        -0.0084211,  0.0231501, -0.9996966,
+      #         0.9999620, -0.0020509, -0.0084708]
+      # Pcl: [-0.0025563, 0.0567484, -0.0512149]
 
-time_offset: 
-  imu_time_offset: 0.0
-  img_time_offset: -0.1
-  exposure_time_init: 0.0
-# ╔═══════════════════════════════════════════════════════════════════════════════════════╗
-# ║                     Configuration Settings                                            ║
-# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
-# ║    Series   │      ID     │ img_time_offset │ exposure_time_init │  -s (start hover)  ║
-# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
-# ║  HKairport  │ HKairport01 │       0.1       │         0.0        │         75         ║
-# ║             │ HKairport02 │      -0.1       │         0.0        │         60         ║
-# ║             │ HKairport03 │      -0.1       │         0.0        │         62         ║
-# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
-# ║  HKisland   │ HKisland01  │       0.0       │         0.0        │        118         ║
-# ║             │ HKisland02  │       0.1       │         0.0        │         80         ║
-# ║             │ HKisland03  │      -0.1       │         0.0        │         72         ║
-# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
-# ║  AMtown     │ AMtown01    │      -0.1       │       0.0285       │         75         ║
-# ║             │ AMtown02    │      -0.1       │       0.0285       │         50         ║
-# ║             │ AMtown03    │      -0.1       │       0.0285       │        106         ║
-# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
-# ║  AMvalley   │ AMvalley01  │      -0.1       │       0.0132       │         70         ║
-# ║             │ AMvalley02  │      -0.1       │       0.0132       │         65         ║
-# ║             │ AMvalley03  │      -0.1       │       0.0132       │         68         ║
-# ╚═══════════════════════════════════════════════════════════════════════════════════════╝
-preprocess:
-  point_filter_num: 1
-  filter_size_surf: 0.1
-  lidar_type: 1 # Livox Avia LiDAR
-  scan_line: 6
-  blind: 0.8
+    time_offset: 
+      imu_time_offset: 0.0
+      img_time_offset: -0.1
+      exposure_time_init: 0.0
+    # ╔═══════════════════════════════════════════════════════════════════════════════════════╗
+    # ║                     Configuration Settings                                            ║
+    # ╠═══════════════════════════════════════════════════════════════════════════════════════╣
+    # ║    Series   │      ID     │ img_time_offset │ exposure_time_init │  -s (start hover)  ║
+    # ╠═══════════════════════════════════════════════════════════════════════════════════════╣
+    # ║  HKairport  │ HKairport01 │       0.1       │         0.0        │         75         ║
+    # ║             │ HKairport02 │      -0.1       │         0.0        │         60         ║
+    # ║             │ HKairport03 │      -0.1       │         0.0        │         62         ║
+    # ╠═══════════════════════════════════════════════════════════════════════════════════════╣
+    # ║  HKisland   │ HKisland01  │       0.0       │         0.0        │        118         ║
+    # ║             │ HKisland02  │       0.1       │         0.0        │         80         ║
+    # ║             │ HKisland03  │      -0.1       │         0.0        │         72         ║
+    # ╠═══════════════════════════════════════════════════════════════════════════════════════╣
+    # ║  AMtown     │ AMtown01    │      -0.1       │       0.0285       │         75         ║
+    # ║             │ AMtown02    │      -0.1       │       0.0285       │         50         ║
+    # ║             │ AMtown03    │      -0.1       │       0.0285       │        106         ║
+    # ╠═══════════════════════════════════════════════════════════════════════════════════════╣
+    # ║  AMvalley   │ AMvalley01  │      -0.1       │       0.0132       │         70         ║
+    # ║             │ AMvalley02  │      -0.1       │       0.0132       │         65         ║
+    # ║             │ AMvalley03  │      -0.1       │       0.0132       │         68         ║
+    # ╚═══════════════════════════════════════════════════════════════════════════════════════╝
+    preprocess:
+      point_filter_num: 1
+      filter_size_surf: 0.1
+      lidar_type: 1 # Livox Avia LiDAR
+      scan_line: 6
+      blind: 0.8
 
-vio:
-  max_iterations: 5
-  outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
-  img_point_cov: 1000 # 100 1000 
-  patch_size: 8
-  patch_pyrimid_level: 4
-  normal_en: true
-  raycast_en: false
-  inverse_composition_en: false
-  exposure_estimate_en: true
-  inv_expo_cov: 0.1
+    vio:
+      max_iterations: 5
+      outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
+      img_point_cov: 1000 # 100 1000 
+      patch_size: 8
+      patch_pyrimid_level: 4
+      normal_en: true
+      raycast_en: false
+      inverse_composition_en: false
+      exposure_estimate_en: true
+      inv_expo_cov: 0.1
 
-imu:
-  imu_en: true
-  imu_int_frame: 30
-  acc_cov: 2.0 # 0.5
-  gyr_cov: 0.1 # 0.3
-  b_acc_cov: 0.0001 # 0.1
-  b_gyr_cov: 0.0001 # 0.1
+    imu:
+      imu_en: true
+      imu_int_frame: 30
+      acc_cov: 2.0 # 0.5
+      gyr_cov: 0.1 # 0.3
+      b_acc_cov: 0.0001 # 0.1
+      b_gyr_cov: 0.0001 # 0.1
 
-lio:
-  max_iterations: 5
-  dept_err: 0.02
-  beam_err: 0.05
-  min_eigen_value: 0.005
-  voxel_size: 2.0 # 1.0
-  max_layer: 2
-  max_points_num: 50
-  layer_init_num: [5, 5, 5, 5, 5]
+    lio:
+      max_iterations: 5
+      dept_err: 0.02
+      beam_err: 0.05
+      min_eigen_value: 0.005
+      voxel_size: 2.0 # 1.0
+      max_layer: 2
+      max_points_num: 50
+      layer_init_num: [5, 5, 5, 5, 5]
 
-local_map:  
-  map_sliding_en: false
-  half_map_size: 100
-  sliding_thresh: 8
+    local_map:  
+      map_sliding_en: false
+      half_map_size: 100
+      sliding_thresh: 8.0
 
-uav:
-  imu_rate_odom: false
-  gravity_align_en: false
+    uav:
+      imu_rate_odom: false
+      gravity_align_en: false
 
-publish:
-  dense_map_en: true
-  pub_effect_point_en: false 
-  pub_plane_en: false
-  pub_scan_num: 1
-  blind_rgb_points: 0.0
+    publish:
+      dense_map_en: true
+      pub_effect_point_en: false 
+      pub_plane_en: false
+      pub_scan_num: 1
+      blind_rgb_points: 0.0
 
-evo:
-  seq_name: "HKisland03"
-  pose_output_en: false
+    evo:
+      seq_name: "HKisland03"
+      pose_output_en: false
 
-pcd_save:
-  pcd_save_en: false
-  colmap_output_en: false # need to set interval = -1
-  filter_size_pcd: 0.15
-  interval: -1
-  # how many LiDAR frames saved in each pcd file;
-  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
\ No newline at end of file
+    pcd_save:
+      pcd_save_en: false
+      colmap_output_en: false # need to set interval = -1
+      filter_size_pcd: 0.15
+      interval: -1
+      # how many LiDAR frames saved in each pcd file;
+      # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
\ No newline at end of file
diff --git a/config/avia.yaml b/config/avia.yaml
index 04c24bab..61d39346 100755
--- a/config/avia.yaml
+++ b/config/avia.yaml
@@ -1,85 +1,89 @@
-common:
-  img_topic: "/left_camera/image"
-  lid_topic: "/livox/lidar"
-  imu_topic: "/livox/imu"
-  img_en: 1
-  lidar_en: 1
-  ros_driver_bug_fix: false
+/**:
+  ros__parameters:
+    common:
+      img_topic: "/left_camera/image"
+      lid_topic: "/livox/lidar"
+      imu_topic: "/livox/imu"
+      img_en: 1
+      lidar_en: 1
+      ros_driver_bug_fix: false
 
-extrin_calib:
-  extrinsic_T: [0.04165, 0.02326, -0.0284]
-  extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-  Rcl: [0.00610193,-0.999863,-0.0154172,
-        -0.00615449,0.0153796,-0.999863,
-        0.999962,0.00619598,-0.0060598] 
-  Pcl: [0.0194384, 0.104689,-0.0251952]
+    extrin_calib:
+      extrinsic_T: [0.04165, 0.02326, -0.0284]
+      extrinsic_R: [1.0, 0.0, 0.0,
+                    0.0, 1.0, 0.0,
+                    0.0, 0.0, 1.0]
+      Rcl: [0.00610193,-0.999863,-0.0154172,
+            -0.00615449,0.0153796,-0.999863,
+            0.999962,0.00619598,-0.0060598] 
+      Pcl: [0.0194384, 0.104689,-0.0251952]
 
-time_offset: 
-  imu_time_offset: 0.0
-  img_time_offset: 0.1
-  exposure_time_init: 0.0
+    time_offset: 
+      imu_time_offset: 0.0
+      img_time_offset: 0.1
+      exposure_time_init: 0.0
 
-preprocess:
-  point_filter_num: 1
-  filter_size_surf: 0.1
-  lidar_type: 1 # Livox Avia LiDAR
-  scan_line: 6
-  blind: 0.8
+    preprocess:
+      point_filter_num: 1
+      filter_size_surf: 0.1
+      lidar_type: 1 # Livox Avia LiDAR
+      scan_line: 6
+      blind: 0.8
 
-vio:
-  max_iterations: 5
-  outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
-  img_point_cov: 100 # 100 1000 
-  patch_size: 8
-  patch_pyrimid_level: 4
-  normal_en: true
-  raycast_en: false
-  inverse_composition_en: false
-  exposure_estimate_en: true
-  inv_expo_cov: 0.1
+    vio:
+      max_iterations: 5
+      outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
+      img_point_cov: 100 # 100 1000 
+      patch_size: 8
+      patch_pyrimid_level: 4
+      normal_en: true
+      raycast_en: false
+      inverse_composition_en: false
+      exposure_estimate_en: true
+      inv_expo_cov: 0.1
 
-imu:
-  imu_en: true
-  imu_int_frame: 30
-  acc_cov: 0.5 # 0.2
-  gyr_cov: 0.3 # 0.5
-  b_acc_cov: 0.0001 # 0.1
-  b_gyr_cov: 0.0001 # 0.1
+    imu:
+      imu_en: true
+      imu_int_frame: 30
+      acc_cov: 0.5 # 0.2
+      gyr_cov: 0.3 # 0.5
+      b_acc_cov: 0.0001 # 0.1
+      b_gyr_cov: 0.0001 # 0.1
 
-lio:
-  max_iterations: 5
-  dept_err: 0.02
-  beam_err: 0.05
-  min_eigen_value: 0.0025 # 0.005
-  voxel_size: 0.5
-  max_layer: 2
-  max_points_num: 50
-  layer_init_num: [5, 5, 5, 5, 5]
+    lio:
+      max_iterations: 5
+      dept_err: 0.02
+      beam_err: 0.05
+      min_eigen_value: 0.0025 # 0.005
+      voxel_size: 0.5
+      max_layer: 2
+      max_points_num: 50
+      layer_init_num: [5, 5, 5, 5, 5]
 
-local_map:  
-  map_sliding_en: false
-  half_map_size: 100
-  sliding_thresh: 8
+    local_map:  
+      map_sliding_en: false
+      half_map_size: 100
+      sliding_thresh: 8.0
 
-uav:
-  imu_rate_odom: false
-  gravity_align_en: false
+    uav:
+      imu_rate_odom: false
+      gravity_align_en: false
 
-publish:
-  dense_map_en: true
-  pub_effect_point_en: false 
-  pub_plane_en: false
-  pub_scan_num: 1
-  blind_rgb_points: 0.0
+    publish:
+      dense_map_en: true
+      pub_effect_point_en: false 
+      pub_plane_en: false
+      pub_scan_num: 1
+      blind_rgb_points: 0.0
 
-evo:
-  seq_name: "CBD_Building_01"
-  pose_output_en: false
+    evo:
+      seq_name: "CBD_Building_01"
+      pose_output_en: false
 
-pcd_save:
-  pcd_save_en: false
-  colmap_output_en: false # need to set interval = -1
-  filter_size_pcd: 0.15
-  interval: -1
-  # how many LiDAR frames saved in each pcd file;
-  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
+    pcd_save:
+      pcd_save_en: false
+      colmap_output_en: false # need to set interval = -1
+      filter_size_pcd: 0.15
+      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.
diff --git a/config/camera_MARS_LVIG.yaml b/config/camera_MARS_LVIG.yaml
index 698d6484..aa48b859 100644
--- a/config/camera_MARS_LVIG.yaml
+++ b/config/camera_MARS_LVIG.yaml
@@ -1,26 +1,29 @@
-cam_model: Pinhole
-# HKisland  HKairport
-cam_width: 2448
-cam_height: 2048
-scale: 0.25
-cam_fx: 1444.431662789634           
-cam_fy: 1444.343536688358
-cam_cx: 1177.801079401826
-cam_cy: 1043.601026568268
-cam_d0: -0.05729528706141188
-cam_d1: 0.1210407244166642
-cam_d2: 0.001274128378760289
-cam_d3: 0.0004389741530109464
+/**:
+  ros__parameters:
+    camera:
+      model: Pinhole
+      # HKisland  HKairport
+      width: 2448
+      height: 2048
+      scale: 0.25
+      fx: 1444.431662789634
+      fy: 1444.343536688358
+      cx: 1177.801079401826
+      cy: 1043.601026568268
+      d0: -0.05729528706141188
+      d1: 0.1210407244166642
+      d2: 0.001274128378760289
+      d3: 0.0004389741530109464
 
-# AMtown AMvalley
-# cam_width: 2448
-# cam_height: 2048
-# scale: 0.25
-# cam_fx: 1453.88
-# cam_fy: 1452.85
-# cam_cx: 1182.53
-# cam_cy: 1045.82 
-# cam_d0: -0.052
-# cam_d1: 0.1168
-# cam_d2: 0.0015
-# cam_d3: 0.00013
\ No newline at end of file
+      # AMtown AMvalley
+      # width: 2448
+      # height: 2048
+      # scale: 0.25
+      # fx: 1453.88
+      # fy: 1452.85
+      # cx: 1182.53
+      # cy: 1045.82
+      # d0: -0.052
+      # d1: 0.1168
+      # d2: 0.0015
+      # d3: 0.00013
diff --git a/config/camera_pinhole.yaml b/config/camera_pinhole.yaml
index 83fd89e0..e416c89c 100644
--- a/config/camera_pinhole.yaml
+++ b/config/camera_pinhole.yaml
@@ -1,12 +1,15 @@
-cam_model: Pinhole
-cam_width: 1280
-cam_height: 1024
-scale: 0.5
-cam_fx: 1293.56944
-cam_fy: 1293.3155
-cam_cx: 626.91359
-cam_cy: 522.799224
-cam_d0: -0.076160
-cam_d1: 0.123001
-cam_d2: -0.00113
-cam_d3: 0.000251
\ No newline at end of file
+/**:
+  ros__parameters:
+    camera:
+      model: Pinhole
+      width: 1280
+      height: 1024
+      scale: 0.5
+      fx: 1293.56944
+      fy: 1293.3155
+      cx: 626.91359
+      cy: 522.799224
+      d0: -0.076160
+      d1: 0.123001
+      d2: -0.00113
+      d3: 0.000251
diff --git a/include/IMU_Processing.h b/include/IMU_Processing.h
index 6774142a..2a045fa6 100644
--- a/include/IMU_Processing.h
+++ b/include/IMU_Processing.h
@@ -14,9 +14,10 @@ which is included as part of this source code package.
 #define IMU_PROCESSING_H
 
 #include <Eigen/Eigen>
+#include <fstream>
 #include "common_lib.h"
 #include <condition_variable>
-#include <nav_msgs/Odometry.h>
+#include <nav_msgs/msg/odometry.hpp>
 #include <utils/so3_math.h>
 
 const bool time_list(PointType &x,
@@ -32,7 +33,7 @@ class ImuProcess
   ~ImuProcess();
 
   void Reset();
-  void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
+  void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu);
   void set_extrinsic(const V3D &transl, const M3D &rot);
   void set_extrinsic(const V3D &transl);
   void set_extrinsic(const MD(4, 4) & T);
@@ -69,7 +70,7 @@ class ImuProcess
   void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N);
   void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out);
   PointCloudXYZI pcl_wait_proc;
-  sensor_msgs::ImuConstPtr last_imu;
+  sensor_msgs::msg::Imu::ConstSharedPtr last_imu;
   PointCloudXYZI::Ptr cur_pcl_un_;
   vector<Pose6D> IMUpose;
   M3D Lid_rot_to_IMU;
diff --git a/include/LIVMapper.h b/include/LIVMapper.h
index 73a562d6..941d5de6 100644
--- a/include/LIVMapper.h
+++ b/include/LIVMapper.h
@@ -16,20 +16,26 @@ which is included as part of this source code package.
 #include "IMU_Processing.h"
 #include "vio.h"
 #include "preprocess.h"
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
-#include <image_transport/image_transport.h>
-#include <nav_msgs/Path.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
+#include <image_transport/image_transport.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/path.hpp>
 #include <vikit/camera_loader.h>
 
 class LIVMapper
 {
 public:
-  LIVMapper(ros::NodeHandle &nh);
+  LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   ~LIVMapper();
-  void initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it);
-  void initializeComponents();
+  void initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &nh, image_transport::ImageTransport &it_);
+  void initializeComponents(rclcpp::Node::SharedPtr &node);
   void initializeFiles();
-  void run();
+  void run(rclcpp::Node::SharedPtr &node);
   void gravityAlignment();
   void handleFirstFrame();
   void stateEstimationAndMapping();
@@ -40,27 +46,27 @@ class LIVMapper
   
   bool sync_packages(LidarMeasureGroup &meas);
   void prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr);
-  void imu_prop_callback(const ros::TimerEvent &e);
+  void imu_prop_callback();
   void transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud);
   void pointBodyToWorld(const PointType &pi, PointType &po);
  
   void RGBpointBodyToWorld(PointType const *const pi, PointType *const po);
-  void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg);
-  void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in);
-  void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in);
-  void img_cbk(const sensor_msgs::ImageConstPtr &msg_in);
+  void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in);
+  void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in);
+  void img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in);
   void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager);
-  void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager);
-  void publish_visual_sub_map(const ros::Publisher &pubSubVisualMap);
-  void publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector<PointToPlane> &ptpl_list);
-  void publish_odometry(const ros::Publisher &pubOdomAftMapped);
-  void publish_mavros(const ros::Publisher &mavros_pose_publisher);
-  void publish_path(const ros::Publisher pubPath);
-  void readParameters(ros::NodeHandle &nh);
+  void publish_frame_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager);
+  void publish_visual_sub_map(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubSubVisualMap);
+  void publish_effect_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudEffect, const std::vector<PointToPlane> &ptpl_list);
+  void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr &pmavros_pose_publisherubOdomAftMapped);
+  void publish_mavros(const rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr &mavros_pose_publisher);
+  void publish_path(const rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr &pubPath);
+  void readParameters(rclcpp::Node::SharedPtr &node);
   template <typename T> void set_posestamp(T &out);
   template <typename T> void pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi, Eigen::Matrix<T, 3, 1> &po);
   template <typename T> Eigen::Matrix<T, 3, 1> pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi);
-  cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg);
+  cv::Mat getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg);
 
   std::mutex mtx_buffer, mtx_buffer_imu_prop;
   std::condition_variable sig_buffer;
@@ -91,11 +97,11 @@ class LIVMapper
   StatesGroup imu_propagate, latest_ekf_state;
 
   bool new_imu = false, state_update_flg = false, imu_prop_enable = true, ekf_finish_once = false;
-  deque<sensor_msgs::Imu> prop_imu_buffer;
-  sensor_msgs::Imu newest_imu;
+  deque<sensor_msgs::msg::Imu> prop_imu_buffer;
+  sensor_msgs::msg::Imu newest_imu;
   double latest_ekf_time;
-  nav_msgs::Odometry imu_prop_odom;
-  ros::Publisher pubImuPropOdom;
+  nav_msgs::msg::Odometry imu_prop_odom;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubImuPropOdom;
   double imu_time_offset = 0.0;
 
   bool gravity_align_en = false, gravity_align_finished = false;
@@ -113,13 +119,13 @@ class LIVMapper
   int lidar_en = 1;
   bool is_first_frame = false;
   int grid_size, patch_size, grid_n_width, grid_n_height, patch_pyrimid_level;
-  double outlier_threshold;
+  int outlier_threshold;
   double plot_time;
   int frame_cnt;
   double img_time_offset = 0.0;
   deque<PointCloudXYZI::Ptr> lid_raw_data_buffer;
   deque<double> lid_header_time_buffer;
-  deque<sensor_msgs::Imu::ConstPtr> imu_buffer;
+  deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu_buffer;
   deque<cv::Mat> img_buffer;
   deque<double> img_time_buffer;
   vector<pointWithVar> _pv_list;
@@ -127,7 +133,7 @@ class LIVMapper
   vector<double> extrinR;
   vector<double> cameraextrinT;
   vector<double> cameraextrinR;
-  double IMG_POINT_COV;
+  int IMG_POINT_COV;
 
   PointCloudXYZI::Ptr visual_sub_map;
   PointCloudXYZI::Ptr feats_undistort;
@@ -148,34 +154,35 @@ class LIVMapper
   StatesGroup _state;
   StatesGroup  state_propagat;
 
-  nav_msgs::Path path;
-  nav_msgs::Odometry odomAftMapped;
-  geometry_msgs::Quaternion geoQuat;
-  geometry_msgs::PoseStamped msg_body_pose;
+  nav_msgs::msg::Path path;
+  nav_msgs::msg::Odometry odomAftMapped;
+  geometry_msgs::msg::Quaternion geoQuat;
+  geometry_msgs::msg::PoseStamped msg_body_pose;
 
   PreprocessPtr p_pre;
   ImuProcessPtr p_imu;
   VoxelMapManagerPtr voxelmap_manager;
   VIOManagerPtr vio_manager;
 
-  ros::Publisher plane_pub;
-  ros::Publisher voxel_pub;
-  ros::Subscriber sub_pcl;
-  ros::Subscriber sub_imu;
-  ros::Subscriber sub_img;
-  ros::Publisher pubLaserCloudFullRes;
-  ros::Publisher pubNormal;
-  ros::Publisher pubSubVisualMap;
-  ros::Publisher pubLaserCloudEffect;
-  ros::Publisher pubLaserCloudMap;
-  ros::Publisher pubOdomAftMapped;
-  ros::Publisher pubPath;
-  ros::Publisher pubLaserCloudDyn;
-  ros::Publisher pubLaserCloudDynRmed;
-  ros::Publisher pubLaserCloudDynDbg;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_pub;
+  std::shared_ptr<rclcpp::SubscriptionBase> sub_pcl;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_img;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubNormal;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSubVisualMap;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDyn;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynRmed;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynDbg;
   image_transport::Publisher pubImage;
-  ros::Publisher mavros_pose_publisher;
-  ros::Timer imu_prop_timer;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr mavros_pose_publisher;
+  rclcpp::TimerBase::SharedPtr imu_prop_timer;
+  rclcpp::Node::SharedPtr node;
 
   int frame_num = 0;
   double aver_time_consu = 0;
@@ -183,4 +190,4 @@ class LIVMapper
   double aver_time_map_inre = 0;
   bool colmap_output_en = false;
 };
-#endif
\ No newline at end of file
+#endif
diff --git a/include/common_lib.h b/include/common_lib.h
index 7e72ea73..a3f951d5 100755
--- a/include/common_lib.h
+++ b/include/common_lib.h
@@ -16,13 +16,16 @@ which is included as part of this source code package.
 #include <utils/so3_math.h>
 #include <utils/types.h>
 #include <utils/color.h>
+#include <utils/utils.h>
 #include <opencv2/opencv.hpp>
-#include <sensor_msgs/Imu.h>
-#include <sophus/se3.h>
-#include <tf/transform_broadcaster.h>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sophus/se3.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Transform.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
 
 using namespace std;
-using namespace Eigen;
+// using namespace Eigen;   // avoid cmake error: reference to ‘Matrix’ is ambiguous
 using namespace Sophus;
 
 #define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl;
@@ -62,7 +65,7 @@ struct MeasureGroup
 {
   double vio_time;
   double lio_time;
-  deque<sensor_msgs::Imu::ConstPtr> imu;
+  deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu;
   cv::Mat img;
   MeasureGroup()
   {
diff --git a/include/feature.h b/include/feature.h
index 08a17f87..a94eea74 100644
--- a/include/feature.h
+++ b/include/feature.h
@@ -33,13 +33,13 @@ struct Feature
   int level_;            //!< Image pyramid level where patch feature was extracted.
   VisualPoint *point_;   //!< Pointer to 3D point which corresponds to the patch feature.
   Vector2d grad_;        //!< Dominant gradient direction for edglets, normalized.
-  SE3 T_f_w_;            //!< Pose of the frame where the patch feature was extracted.
+  SE3<double> T_f_w_;            //!< Pose of the frame where the patch feature was extracted.
   float *patch_;         //!< Pointer to the image patch data.
   float score_;          //!< Score of the patch feature.
   float mean_;           //!< Mean intensity of the image patch feature, used for normalization.
   double inv_expo_time_; //!< Inverse exposure time of the image where the patch feature was extracted.
   
-  Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3 &_T_f_w, int _level)
+  Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3<double> &_T_f_w, int _level)
       : type_(CORNER), px_(_px), f_(_f), T_f_w_(_T_f_w), mean_(0), score_(0), level_(_level), patch_(_patch), point_(_point)
   {
   }
diff --git a/include/frame.h b/include/frame.h
index 8172dcdc..ea8a534d 100644
--- a/include/frame.h
+++ b/include/frame.h
@@ -31,8 +31,8 @@ class Frame : boost::noncopyable
   static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id.
   int id_;                   //!< Unique id of the frame.
   vk::AbstractCamera *cam_;  //!< Camera model.
-  SE3 T_f_w_;                //!< Transform (f)rame from (w)orld.
-  SE3 T_f_w_prior_;          //!< Transform (f)rame from (w)orld provided by the IMU prior.
+  SE3<double> T_f_w_;                //!< Transform (f)rame from (w)orld.
+  SE3<double> T_f_w_prior_;          //!< Transform (f)rame from (w)orld provided by the IMU prior.
   cv::Mat img_;              //!< Image of the frame.
   Features fts_;             //!< List of features in the image.
 
diff --git a/include/preprocess.h b/include/preprocess.h
index 649ed838..98303037 100755
--- a/include/preprocess.h
+++ b/include/preprocess.h
@@ -14,7 +14,7 @@ which is included as part of this source code package.
 #define PREPROCESS_H_
 
 #include "common_lib.h"
-#include <livox_ros_driver/CustomMsg.h>
+#include <livox_ros_driver2/msg/custom_msg.hpp>
 #include <pcl_conversions/pcl_conversions.h>
 
 using namespace std;
@@ -70,13 +70,13 @@ struct EIGEN_ALIGN16 Point
 {
   PCL_ADD_POINT4D;
   float intensity;
-  std::uint32_t t;
+  float time;
   std::uint16_t ring;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 } // namespace velodyne_ros
 POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
-                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, t, t)(std::uint16_t, ring, ring))
+                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring))
 /****************/
 
 /*** Ouster ***/
@@ -138,29 +138,31 @@ class Preprocess
   Preprocess();
   ~Preprocess();
 
-  void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
-  void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
+  void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &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;
+  // sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud;
   PointCloudXYZI pl_full, pl_corn, pl_surf;
   PointCloudXYZI pl_buff[128]; // maximum 128 line lidar
   vector<orgtype> typess[128]; // maximum 128 line lidar
-  int lidar_type, point_filter_num, N_SCANS;
+  int lidar_type, point_filter_num, N_SCANS, SCAN_RATE;
   
   double blind, blind_sqr;
   bool feature_enabled, given_offset_time;
-  ros::Publisher pub_full, pub_surf, pub_corn;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_full;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_surf;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_corn;
 
 private:
-  void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
-  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
-  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
-  void xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
-  void Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
-  void l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg);
+  void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void l515_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);
diff --git a/include/utils/utils.h b/include/utils/utils.h
new file mode 100644
index 00000000..9555f705
--- /dev/null
+++ b/include/utils/utils.h
@@ -0,0 +1,76 @@
+#ifndef UTILS_H
+#define UTILS_H
+
+#include <vector>
+#include <cstdint> // for int64_t
+#include <limits>  // for std::numeric_limits
+#include <stdexcept> // for std::out_of_range
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+
+std::vector<int> convertToIntVectorSafe(const std::vector<int64_t>& int64_vector);
+
+inline double stamp2Sec(const builtin_interfaces::msg::Time& stamp)
+{
+    return rclcpp::Time(stamp).seconds();
+}
+
+inline rclcpp::Time sec2Stamp(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);
+}
+
+namespace tf
+{
+
+inline geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw)
+{
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw);
+    return tf2::toMsg(q);
+}
+
+inline geometry_msgs::msg::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
+{
+    tf2::Quaternion q;
+    q.setRPY(roll, pitch, yaw);
+    return tf2::toMsg(q);
+}
+
+inline tf2::Quaternion createQuaternionFromYaw(double yaw)
+{
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw);
+    return q;
+}
+
+inline tf2::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
+{
+    tf2::Quaternion q;
+    q.setRPY(roll, pitch, yaw);
+    return q;
+}
+}
+
+inline geometry_msgs::msg::TransformStamped createTransformStamped(
+    const tf2::Transform &transform,
+    const builtin_interfaces::msg::Time &stamp,
+    const std::string &frame_id,
+    const std::string &child_frame_id)
+{
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped.header.stamp = stamp;
+    transform_stamped.header.frame_id = frame_id;
+    transform_stamped.child_frame_id = child_frame_id;
+    transform_stamped.transform = tf2::toMsg(transform);
+    return transform_stamped;
+}
+
+#endif // UTILS_H
\ No newline at end of file
diff --git a/include/vio.h b/include/vio.h
index d5600b1d..21f2d9df 100755
--- a/include/vio.h
+++ b/include/vio.h
@@ -119,8 +119,8 @@ class VIOManager
   int frame_count = 0;
   bool plot_flag;
 
-  Matrix<double, DIM_STATE, DIM_STATE> G, H_T_H;
-  MatrixXd K, H_sub_inv;
+  Eigen::Matrix<double, DIM_STATE, DIM_STATE> G, H_T_H;
+  Eigen::MatrixXd K, H_sub_inv;
 
   ofstream fout_camera, fout_colmap;
   unordered_map<VOXEL_LOCATION, VOXEL_POINTS *> feat_map;
@@ -153,11 +153,11 @@ class VIOManager
   void computeJacobianAndUpdateEKF(cv::Mat img);
   void resetGrid();
   void updateVisualMapPoints(cv::Mat img);
-  void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref,
+  void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3<double> &T_cur_ref,
                            const int level_ref, 
                            const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref);
   void getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref,
-                                     const V3D &xyz_ref, const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref);
+                                     const V3D &xyz_ref, const V3D &normal_ref, const SE3<double> &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref);
   void warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level,
                   const int pyramid_level, const int halfpatch_size, float *patch);
   void insertPointIntoVoxelMap(VisualPoint *pt_new);
diff --git a/include/voxel_map.h b/include/voxel_map.h
index 21afaa34..2f28d80e 100644
--- a/include/voxel_map.h
+++ b/include/voxel_map.h
@@ -20,12 +20,12 @@ which is included as part of this source code package.
 #include <mutex>
 #include <omp.h>
 #include <pcl/common/io.h>
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <thread>
 #include <unistd.h>
 #include <unordered_map>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #define VOXELMAP_HASH_P 116101
 #define VOXELMAP_MAX_N 10000000000
@@ -37,7 +37,7 @@ typedef struct VoxelMapConfig
   double max_voxel_size_;
   int max_layer_;
   int max_iterations_;
-  std::vector<int> layer_init_num_;
+  std::vector<int64_t> layer_init_num_;
   int max_points_num_;
   double planner_threshold_;
   double beam_err_;
@@ -182,7 +182,7 @@ class VoxelOctoTree
   VoxelOctoTree *Insert(const pointWithVar &pv);
 };
 
-void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config);
+void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config);
 
 class VoxelMapManager
 {
@@ -190,7 +190,7 @@ class VoxelMapManager
   VoxelMapManager() = default;
   VoxelMapConfig config_setting_;
   int current_frame_id_ = 0;
-  ros::Publisher voxel_map_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_map_pub_;
   std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> voxel_map_;
 
   PointCloudXYZI::Ptr feats_undistort_;
@@ -208,7 +208,7 @@ class VoxelMapManager
 
   V3D last_slide_position = {0,0,0};
 
-  geometry_msgs::Quaternion geoQuat_;
+  geometry_msgs::msg::Quaternion geoQuat_;
 
   int feats_down_size_;
   int effct_feat_num_;
@@ -248,9 +248,9 @@ class VoxelMapManager
 private:
   void GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector<VoxelPlane> &plane_list);
 
-  void pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha,
+  void pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha,
                       const Eigen::Vector3d rgb);
-  void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::Quaternion &q);
+  void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::msg::Quaternion &q);
 
   void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b);
 };
diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch
deleted file mode 100755
index 1e5c3602..00000000
--- a/launch/mapping_avia.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-<!-- Launch file for Livox AVIA LiDAR -->
-
-	<arg name="rviz" default="true" />
-
-	<rosparam command="load" file="$(find fast_livo)/config/avia.yaml" />
-
-	<node pkg="fast_livo" type="fastlivo_mapping" name="laserMapping" output="screen">
-		<rosparam file="$(find fast_livo)/config/camera_pinhole.yaml" />
-	</node>
-	
-	<group if="$(arg rviz)">
-		<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_livo)/rviz_cfg/fast_livo2.rviz" />
-	</group>
-	
-	<node pkg="image_transport" type="republish" name="republish" args="compressed in:=/left_camera/image raw out:=/left_camera/image" output="screen" respawn="true"/>
-
-launch-prefix="gdb -ex run --args"
-launch-prefix="valgrind --leak-check=full"
-</launch> 	
diff --git a/launch/mapping_avia.launch.py b/launch/mapping_avia.launch.py
new file mode 100644
index 00000000..ab768309
--- /dev/null
+++ b/launch/mapping_avia.launch.py
@@ -0,0 +1,106 @@
+#!/usr/bin/python3
+# -- coding: utf-8 --**
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    
+    # Find path
+    config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config")
+    rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "fast_livo2.rviz")
+
+    #Load parameters
+    avia_config_cmd = os.path.join(config_file_dir, "avia.yaml")
+    camera_config_cmd = os.path.join(config_file_dir, "camera_pinhole.yaml")
+
+    # Param use_rviz
+    use_rviz_arg = DeclareLaunchArgument(
+        "use_rviz",
+        default_value="False",
+        description="Whether to launch Rviz2",
+    )
+
+    avia_config_arg = DeclareLaunchArgument(
+        'avia_params_file',
+        default_value=avia_config_cmd,
+        description='Full path to the ROS2 parameters file to use for fast_livo2 nodes',
+    )
+
+    camera_config_arg = DeclareLaunchArgument(
+        'camera_params_file',
+        default_value=camera_config_cmd,
+        description='Full path to the ROS2 parameters file to use for vikit_ros nodes',
+    )
+
+    # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40
+    use_respawn_arg = DeclareLaunchArgument(
+        'use_respawn', 
+        default_value='True',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    avia_params_file = LaunchConfiguration('avia_params_file')
+    camera_params_file = LaunchConfiguration('camera_params_file')
+    use_respawn = LaunchConfiguration('use_respawn')
+
+    return LaunchDescription([
+        use_rviz_arg,
+        avia_config_arg,
+        camera_config_arg,
+        use_respawn_arg,
+
+        # play ros2 bag
+        # ExecuteProcess(
+        #     cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], 
+        #     shell=True
+        # ),
+
+        # republish compressed image to raw image
+        # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2
+        # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image
+        Node(
+            package="image_transport",
+            executable="republish",
+            name="republish",
+            arguments=[ # Array of strings/parametric arguments that will end up in process's argv
+                'compressed', 
+                'raw',
+            ],
+            remappings=[
+                ("in",  "/left_camera/image"), 
+                ("out", "/left_camera/image")
+            ],
+            output="screen",
+            respawn=use_respawn,
+        ),
+        
+        Node(
+            package="fast_livo",
+            executable="fastlivo_mapping",
+            name="laserMapping",
+            parameters=[
+                avia_params_file,
+                camera_params_file,
+            ],
+            # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html
+            prefix=[
+                # ("gdb -ex run --args"),
+                # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes")
+            ],
+            output="screen"
+        ),
+
+        Node(
+            condition=IfCondition(LaunchConfiguration("use_rviz")),
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            arguments=["-d", rviz_config_file],
+            output="screen"
+        ),
+    ])
diff --git a/launch/mapping_avia_marslvig.launch b/launch/mapping_avia_marslvig.launch
deleted file mode 100755
index 2fa90d3d..00000000
--- a/launch/mapping_avia_marslvig.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-<!-- Launch file for Livox AVIA LiDAR -->
-
-	<arg name="rviz" default="true" />
-
-	<rosparam command="load" file="$(find fast_livo)/config/MARS_LVIG.yaml" />
-
-	<node pkg="fast_livo" type="fastlivo_mapping" name="laserMapping" output="screen">
-		<rosparam file="$(find fast_livo)/config/camera_MARS_LVIG.yaml" />
-	</node>
-	
-	<group if="$(arg rviz)">
-		<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_livo)/rviz_cfg/M300.rviz" />
-	</group>
-	
-	<node pkg="image_transport" type="republish" name="republish" args="compressed in:=/left_camera/image raw out:=/left_camera/image" output="screen" respawn="true"/>
-
-launch-prefix="gdb -ex run --args"
-launch-prefix="valgrind --leak-check=full"
-</launch> 	
diff --git a/launch/mapping_avia_marslvig.launch.py b/launch/mapping_avia_marslvig.launch.py
new file mode 100755
index 00000000..7e787500
--- /dev/null
+++ b/launch/mapping_avia_marslvig.launch.py
@@ -0,0 +1,111 @@
+#!/usr/bin/python3
+# -- coding: utf-8 --**
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    
+    # Find path
+    config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config")
+    rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "M300.rviz")
+
+    #Load parameters
+    avia_config_cmd = os.path.join(config_file_dir, "MARS_LVIG.yaml")
+    camera_config_cmd = os.path.join(config_file_dir, "camera_MARS_LVIG.yaml")
+
+    # Param use_rviz
+    use_rviz_arg = DeclareLaunchArgument(
+        "use_rviz",
+        default_value="False",
+        description="Whether to launch Rviz2",
+    )
+
+    avia_config_arg = DeclareLaunchArgument(
+        'avia_params_file',
+        default_value=avia_config_cmd,
+        description='Full path to the ROS2 parameters file to use for fast_livo2 nodes',
+    )
+
+    camera_config_arg = DeclareLaunchArgument(
+        'camera_params_file',
+        default_value=camera_config_cmd,
+        description='Full path to the ROS2 parameters file to use for vikit_ros nodes',
+    )
+
+    # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40
+    use_respawn_arg = DeclareLaunchArgument(
+        'use_respawn', 
+        default_value='True',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    avia_params_file = LaunchConfiguration('avia_params_file')
+    camera_params_file = LaunchConfiguration('camera_params_file')
+    use_respawn = LaunchConfiguration('use_respawn')
+
+    return LaunchDescription([
+        use_rviz_arg,
+        avia_config_arg,
+        camera_config_arg,
+        use_respawn_arg,
+
+        # use parameter_blackboard as global parameters server and load camera params
+        Node(
+            package='demo_nodes_cpp',
+            executable='parameter_blackboard',
+            name='parameter_blackboard',
+            # namespace='laserMapping',
+            parameters=[
+                camera_params_file,
+            ],
+            output='screen'
+        ),
+
+        # republish compressed image to raw image
+        # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2
+        # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image
+        Node(
+            package="image_transport",
+            executable="republish",
+            name="republish",
+            arguments=[ # Array of strings/parametric arguments that will end up in process's argv
+                'compressed', 
+                'raw',
+            ],
+            remappings=[
+                ("in",  "/left_camera/image"), 
+                ("out", "/left_camera/image")
+            ],
+            output="screen",
+            respawn=use_respawn,
+        ),
+        
+        Node(
+            package="fast_livo",
+            executable="fastlivo_mapping",
+            name="laserMapping",
+            parameters=[
+                avia_params_file,
+            ],
+            # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html
+            prefix=[
+                # ("gdb -ex run --args"),
+                # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes")
+            ],
+            output="screen"
+        ),
+
+        Node(
+            condition=IfCondition(LaunchConfiguration("use_rviz")),
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            arguments=["-d", rviz_config_file],
+            output="screen"
+        ),
+    ])
diff --git a/package.xml b/package.xml
index af3a353c..84a4020e 100755
--- a/package.xml
+++ b/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>fast_livo</name>
   <version>0.0.0</version>
 
@@ -16,40 +16,39 @@
 
   <author email="zhangji@cmu.edu">Ji Zhang</author>
   
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclpy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>tf</build_depend>
+  <build_depend>tf2_ros</build_depend>
   <build_depend>pcl_ros</build_depend>
-  <build_depend>livox_ros_driver</build_depend>
-  <build_depend>message_generation</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>livox_ros_driver2</build_depend>
   <build_depend>vikit_common</build_depend>
   <build_depend>vikit_ros</build_depend>
   <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>libopencv-dev</build_depend>
+
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>image_transport</exec_depend>
+  <exec_depend>libopencv-dev</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>pcl_ros</run_depend>
-  <run_depend>livox_ros_driver</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>vikit_common</run_depend>
-  <run_depend>vikit_ros</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <build_depend>image_transport</build_depend> 
-  <run_depend>image_transport</run_depend> 
-
-  <test_depend>rostest</test_depend>
-  <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>
+    <!-- https://github.com/ros-perception/image_transport_tutorials/blob/humble/package.xml -->
+    <!-- <image_transport plugin="${prefix}/resized_plugins.xml"/> -->
   </export>
 </package>
diff --git a/pics/debug_error.png b/pics/debug_error.png
new file mode 100644
index 00000000..3baa5215
Binary files /dev/null and b/pics/debug_error.png differ
diff --git a/pics/rosgraph.png b/pics/rosgraph.png
new file mode 100644
index 00000000..26ba96fd
Binary files /dev/null and b/pics/rosgraph.png differ
diff --git a/rviz_cfg/M300.rviz b/rviz_cfg/M300.rviz
index a5049637..84a33a66 100755
--- a/rviz_cfg/M300.rviz
+++ b/rviz_cfg/M300.rviz
@@ -1,5 +1,5 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
     Help Height: 0
     Name: Displays
     Property Tree Widget:
@@ -25,21 +25,21 @@ Panels:
         - /Image1
       Splitter Ratio: 0.34272301197052
     Tree Height: 538
-  - Class: rviz/Selection
+  - Class: rviz_common/Selection
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
       - /2D Pose Estimate1
       - /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
     Name: Time
     SyncMode: 0
     SyncSource: surround
@@ -52,7 +52,7 @@ Visualization Manager:
   Displays:
     - Alpha: 1
       Cell Size: 1
-      Class: rviz/Grid
+      Class: rviz_default_plugins/Grid
       Color: 160; 160; 164
       Enabled: false
       Line Style:
@@ -69,7 +69,7 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: false
     - Alpha: 1
-      Class: rviz/Axes
+      Class: rviz_default_plugins/Axes
       Enabled: true
       Length: 4
       Name: Axes
@@ -77,7 +77,7 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Show Trail: false
       Value: true
-    - Class: rviz/Group
+    - Class: rviz_common/Group
       Displays:
         - Alpha: 1
           Autocompute Intensity Bounds: true
@@ -87,7 +87,7 @@ Visualization Manager:
             Value: false
           Axis: Z
           Channel Name: intensity
-          Class: rviz/PointCloud2
+          Class: rviz_default_plugins/PointCloud2
           Color: 199; 228; 247
           Color Transformer: FlatColor
           Decay Time: 0
@@ -115,7 +115,7 @@ Visualization Manager:
             Value: false
           Axis: Z
           Channel Name: intensity
-          Class: rviz/PointCloud2
+          Class: rviz_default_plugins/PointCloud2
           Color: 238; 238; 236
           Color Transformer: RGB8
           Decay Time: 10000
@@ -145,7 +145,7 @@ Visualization Manager:
             Value: true
           Axis: Z
           Channel Name: intensity
-          Class: rviz/PointCloud2
+          Class: rviz_default_plugins/PointCloud2
           Color: 239; 41; 41
           Color Transformer: ""
           Decay Time: 0
@@ -167,10 +167,10 @@ Visualization Manager:
           Value: false
       Enabled: true
       Name: mapping
-    - Class: rviz/Group
+    - Class: rviz_common/Group
       Displays:
         - Angle Tolerance: 0.009999999776482582
-          Class: rviz/Odometry
+          Class: rviz_default_plugins/Odometry
           Covariance:
             Orientation:
               Alpha: 0.5
@@ -208,7 +208,7 @@ Visualization Manager:
       Name: Odometry
     - Alpha: 0
       Buffer Length: 2
-      Class: rviz/Path
+      Class: rviz_default_plugins/Path
       Color: 25; 255; 255
       Enabled: true
       Head Diameter: 0
@@ -238,7 +238,7 @@ Visualization Manager:
         Value: false
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 239; 41; 41
       Color Transformer: Intensity
       Decay Time: 1000
@@ -258,7 +258,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
-    - Class: rviz/Marker
+    - Class: rviz_default_plugins/Marker
       Enabled: true
       Marker Topic: /planner_normal
       Name: Marker
@@ -266,7 +266,7 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: true
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
       Marker Topic: /voxels
       Name: MarkerArray
@@ -282,7 +282,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 245; 121; 0
       Color Transformer: FlatColor
       Decay Time: 0
@@ -302,7 +302,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
       Marker Topic: /visualization_marker
       Name: MarkerArray
@@ -318,7 +318,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 92; 53; 102
       Color Transformer: FlatColor
       Decay Time: 99999
@@ -346,7 +346,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 115; 210; 22
       Color Transformer: FlatColor
       Decay Time: 0
@@ -374,7 +374,7 @@ Visualization Manager:
         Value: false
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 237; 212; 0
       Color Transformer: FlatColor
       Decay Time: 0
@@ -402,7 +402,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 239; 41; 41
       Color Transformer: FlatColor
       Decay Time: 99999
@@ -430,7 +430,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 92; 53; 102
       Color Transformer: FlatColor
       Decay Time: 0
@@ -451,7 +451,7 @@ Visualization Manager:
       Use rainbow: true
       Value: false
     - Angle Tolerance: 0
-      Class: rviz/Odometry
+      Class: rviz_default_plugins/Odometry
       Covariance:
         Orientation:
           Alpha: 0.5
@@ -485,7 +485,7 @@ Visualization Manager:
       Topic: /aft_mapped_to_init
       Unreliable: false
       Value: false
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
       Marker Topic: /waypoint_planner/visualize
       Name: MarkerArray
@@ -493,7 +493,7 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: false
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: true
       Marker Topic: /fsm_node/visualization/exp_traj
       Name: MarkerArray
@@ -501,7 +501,7 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: true
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
       Marker Topic: /fsm_node/visualization/exp_sfcs
       Name: MarkerArray
@@ -509,7 +509,7 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: false
-    - Class: rviz/Image
+    - Class: rviz_default_plugins/Image
       Enabled: true
       Image Topic: /rgb_img
       Max Value: 1
@@ -529,26 +529,26 @@ Visualization Manager:
     Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+    - Class: rviz_default_plugins/SetInitialPose
       Theta std deviation: 0.2617993950843811
       Topic: /initialpose
       X std deviation: 0.5
       Y std deviation: 0.5
-    - Class: rviz/SetGoal
+    - Class: rviz_default_plugins/SetGoal
       Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
+    - Class: rviz_default_plugins/PublishPoint
       Single click: true
       Topic: /clicked_point
   Value: true
   Views:
     Current:
-      Class: rviz/ThirdPersonFollower
+      Class: rviz_default_plugins/ThirdPersonFollower
       Distance: 582.7694702148438
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -569,7 +569,7 @@ Visualization Manager:
       Target Frame: drone
       Yaw: 3.1317780017852783
     Saved:
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 117.53474426269531
         Enable Stereo Rendering:
           Stereo Eye Separation: 0.05999999865889549
@@ -589,7 +589,7 @@ Visualization Manager:
         Pitch: 0.19539840519428253
         Target Frame: <Fixed Frame>
         Yaw: 0.17540442943572998
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 109.3125
         Enable Stereo Rendering:
           Stereo Eye Separation: 0.05999999865889549
@@ -609,7 +609,7 @@ Visualization Manager:
         Pitch: 0.035398442298173904
         Target Frame: <Fixed Frame>
         Yaw: 5.793589115142822
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 85.65605163574219
         Enable Stereo Rendering:
           Stereo Eye Separation: 0.05999999865889549
@@ -629,7 +629,7 @@ Visualization Manager:
         Pitch: 0.5653983950614929
         Target Frame: <Fixed Frame>
         Yaw: 0.9104044437408447
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 60.1053581237793
         Enable Stereo Rendering:
           Stereo Eye Separation: 0.05999999865889549
diff --git a/rviz_cfg/fast_livo2.rviz b/rviz_cfg/fast_livo2.rviz
index da2892e3..f89870f5 100755
--- a/rviz_cfg/fast_livo2.rviz
+++ b/rviz_cfg/fast_livo2.rviz
@@ -1,9 +1,10 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
     Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Status1
         - /Axes1
         - /mapping1
@@ -16,6 +17,7 @@ Panels:
         - /Odometry1/Odometry1/Shape1
         - /Path1
         - /currPoints1/Autocompute Value Bounds1
+        - /Marker1
         - /MarkerArray1/Namespaces1
         - /currPoints2/Autocompute Value Bounds1
         - /Odometry2/Shape1
@@ -23,36 +25,32 @@ Panels:
         - /MarkerArray4
         - /MarkerArray5
         - /Image1
-      Splitter Ratio: 0.34272301197052
-    Tree Height: 538
-  - Class: rviz/Selection
+      Splitter Ratio: 0.5394402146339417
+    Tree Height: 360
+  - Class: rviz_common/Selection
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
       - /2D Pose Estimate1
-      - /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
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
     - Alpha: 1
       Cell Size: 1
-      Class: rviz/Grid
+      Class: rviz_default_plugins/Grid
       Color: 160; 160; 164
       Enabled: false
       Line Style:
@@ -68,16 +66,14 @@ Visualization Manager:
       Plane Cell Count: 160
       Reference Frame: <Fixed Frame>
       Value: false
-    - Alpha: 1
-      Class: rviz/Axes
+    - Class: rviz_default_plugins/Axes
       Enabled: true
       Length: 0.699999988079071
       Name: Axes
       Radius: 0.10000000149011612
-      Reference Frame: <Fixed Frame>
-      Show Trail: false
+      Reference Frame: camera_init
       Value: true
-    - Class: rviz/Group
+    - Class: rviz_common/Group
       Displays:
         - Alpha: 1
           Autocompute Intensity Bounds: true
@@ -87,23 +83,29 @@ Visualization Manager:
             Value: false
           Axis: Z
           Channel Name: intensity
-          Class: rviz/PointCloud2
+          Class: rviz_default_plugins/PointCloud2
           Color: 239; 41; 41
           Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
+          Max Intensity: 4096
           Min Color: 0; 0; 0
+          Min Intensity: 0
           Name: currPoints
           Position Transformer: XYZ
-          Queue Size: 100000
           Selectable: true
           Size (Pixels): 4
           Size (m): 0.009999999776482582
           Style: Points
-          Topic: /cloud_registered
-          Unreliable: false
+          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: false
           Value: true
@@ -115,7 +117,7 @@ Visualization Manager:
             Value: false
           Axis: Z
           Channel Name: intensity
-          Class: rviz/PointCloud2
+          Class: rviz_default_plugins/PointCloud2
           Color: 238; 238; 236
           Color Transformer: RGB8
           Decay Time: 10000
@@ -127,13 +129,17 @@ Visualization Manager:
           Min Intensity: 5
           Name: surround
           Position Transformer: XYZ
-          Queue Size: 1
           Selectable: false
           Size (Pixels): 1
           Size (m): 0.004999999888241291
           Style: Points
-          Topic: /cloud_registered
-          Unreliable: true
+          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
@@ -145,32 +151,38 @@ Visualization Manager:
             Value: true
           Axis: Z
           Channel Name: intensity
-          Class: rviz/PointCloud2
+          Class: rviz_default_plugins/PointCloud2
           Color: 239; 41; 41
           Color Transformer: ""
           Decay Time: 0
           Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
+          Max Intensity: 4096
           Min Color: 0; 0; 0
+          Min Intensity: 0
           Name: PointCloud2
           Position Transformer: XYZ
-          Queue Size: 10
           Selectable: true
           Size (Pixels): 5
           Size (m): 0.019999999552965164
           Style: Squares
-          Topic: /cloud_effected
-          Unreliable: false
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /cloud_effected
           Use Fixed Frame: true
           Use rainbow: true
           Value: false
       Enabled: true
       Name: mapping
-    - Class: rviz/Group
+    - Class: rviz_common/Group
       Displays:
         - Angle Tolerance: 0.009999999776482582
-          Class: rviz/Odometry
+          Class: rviz_default_plugins/Odometry
           Covariance:
             Orientation:
               Alpha: 0.5
@@ -190,7 +202,6 @@ Visualization Manager:
           Keep: 1
           Name: Odometry
           Position Tolerance: 0.0010000000474974513
-          Queue Size: 10
           Shape:
             Alpha: 1
             Axes Length: 0.5
@@ -201,14 +212,19 @@ Visualization Manager:
             Shaft Length: 0.800000011920929
             Shaft Radius: 0.5
             Value: Axes
-          Topic: /aft_mapped_to_init
-          Unreliable: false
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /aft_mapped_to_init
           Value: true
       Enabled: true
       Name: Odometry
     - Alpha: 0
       Buffer Length: 2
-      Class: rviz/Path
+      Class: rviz_default_plugins/Path
       Color: 25; 255; 255
       Enabled: true
       Head Diameter: 0
@@ -223,12 +239,16 @@ Visualization Manager:
         Z: 0
       Pose Color: 25; 255; 255
       Pose Style: None
-      Queue Size: 10
       Radius: 0.029999999329447746
       Shaft Diameter: 0.4000000059604645
       Shaft Length: 0.4000000059604645
-      Topic: /path
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /path
       Value: true
     - Alpha: 0.10000000149011612
       Autocompute Intensity Bounds: true
@@ -238,41 +258,56 @@ Visualization Manager:
         Value: false
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 239; 41; 41
       Color Transformer: Intensity
       Decay Time: 1000
       Enabled: false
       Invert Rainbow: true
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: currPoints
       Position Transformer: XYZ
-      Queue Size: 1
       Selectable: true
       Size (Pixels): 2
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /cloud_voxel
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_voxel
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
-    - Class: rviz/Marker
+    - Class: rviz_default_plugins/Marker
       Enabled: true
-      Marker Topic: /planner_normal
       Name: Marker
       Namespaces:
         {}
-      Queue Size: 100
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planner_normal
       Value: true
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
-      Marker Topic: /voxels
       Name: MarkerArray
       Namespaces:
         {}
-      Queue Size: 100
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker_array
       Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -282,33 +317,43 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 245; 121; 0
       Color Transformer: FlatColor
       Decay Time: 0
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
-      Queue Size: 1
       Selectable: true
       Size (Pixels): 15
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /cloud_ray_sub_map
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_ray_sub_map
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
-      Marker Topic: /visualization_marker
       Name: MarkerArray
       Namespaces:
         {}
-      Queue Size: 100
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker_array
       Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -318,23 +363,29 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 92; 53; 102
       Color Transformer: FlatColor
       Decay Time: 99999
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
-      Queue Size: 10
       Selectable: true
       Size (Pixels): 10
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /cloud_visual_map
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_visual_map
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
@@ -346,23 +397,29 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 115; 210; 22
       Color Transformer: FlatColor
       Decay Time: 0
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: surround
       Position Transformer: XYZ
-      Queue Size: 1
       Selectable: false
       Size (Pixels): 12
       Size (m): 0.05000000074505806
       Style: Points
-      Topic: /cloud_visual_sub_map
-      Unreliable: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_visual_sub_map
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
@@ -374,23 +431,29 @@ Visualization Manager:
         Value: false
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 237; 212; 0
       Color Transformer: FlatColor
       Decay Time: 0
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: currPoints
       Position Transformer: XYZ
-      Queue Size: 100000
       Selectable: true
       Size (Pixels): 5
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /cloud_sample_points
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_sample_points
       Use Fixed Frame: true
       Use rainbow: false
       Value: false
@@ -402,23 +465,29 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 239; 41; 41
       Color Transformer: FlatColor
       Decay Time: 99999
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 239; 41; 41
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
-      Queue Size: 10
       Selectable: true
       Size (Pixels): 4
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /cloud_visual_map
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_visual_map
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
@@ -430,28 +499,34 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rviz/PointCloud2
+      Class: rviz_default_plugins/PointCloud2
       Color: 92; 53; 102
       Color Transformer: FlatColor
       Decay Time: 0
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
-      Queue Size: 10
       Selectable: true
       Size (Pixels): 20
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /cloud_ray_sub_map_fov
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_ray_sub_map_fov
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
     - Angle Tolerance: 0
-      Class: rviz/Odometry
+      Class: rviz_default_plugins/Odometry
       Covariance:
         Orientation:
           Alpha: 0.5
@@ -471,7 +546,6 @@ Visualization Manager:
       Keep: 1
       Name: Odometry
       Position Tolerance: 0
-      Queue Size: 10
       Shape:
         Alpha: 1
         Axes Length: 0.699999988079071
@@ -482,80 +556,116 @@ Visualization Manager:
         Shaft Length: 1
         Shaft Radius: 0.05000000074505806
         Value: Axes
-      Topic: /aft_mapped_to_init
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /aft_mapped_to_init
       Value: false
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
-      Marker Topic: /waypoint_planner/visualize
       Name: MarkerArray
       Namespaces:
         {}
-      Queue Size: 100
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker_array
       Value: false
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: true
-      Marker Topic: /fsm_node/visualization/exp_traj
       Name: MarkerArray
       Namespaces:
         {}
-      Queue Size: 100
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker_array
       Value: true
-    - Class: rviz/MarkerArray
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: false
-      Marker Topic: /fsm_node/visualization/exp_sfcs
       Name: MarkerArray
       Namespaces:
         {}
-      Queue Size: 100
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker_array
       Value: false
-    - Class: rviz/Image
+    - Class: rviz_default_plugins/Image
       Enabled: true
-      Image Topic: /rgb_img
       Max Value: 1
       Median window: 5
       Min Value: 0
       Name: Image
       Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rgb_img
       Value: true
   Enabled: true
   Global Options:
     Background Color: 0; 0; 0
-    Default Light: true
     Fixed Frame: camera_init
     Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /move_base_simple/goal
+    - 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/ThirdPersonFollower
-      Distance: 25.669620513916016
+      Class: rviz_default_plugins/ThirdPersonFollower
+      Distance: 50.66728210449219
       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: 5.083343982696533
         Y: -9.8687162399292
@@ -565,18 +675,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.10520334541797638
+      Pitch: 0.3547965884208679
       Target Frame: drone
-      Yaw: 3.161787748336792
+      Value: ThirdPersonFollower (rviz_default_plugins)
+      Yaw: 2.5417795181274414
     Saved:
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 117.53474426269531
         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: -35.713138580322266
           Y: 36.932613372802734
@@ -588,15 +698,15 @@ Visualization Manager:
         Near Clip Distance: 0.009999999776482582
         Pitch: 0.19539840519428253
         Target Frame: <Fixed Frame>
+        Value: Orbit (rviz_default_plugins)
         Yaw: 0.17540442943572998
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 109.3125
         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: -22.092714309692383
           Y: 63.322662353515625
@@ -608,15 +718,15 @@ Visualization Manager:
         Near Clip Distance: 0.009999999776482582
         Pitch: 0.035398442298173904
         Target Frame: <Fixed Frame>
+        Value: Orbit (rviz_default_plugins)
         Yaw: 5.793589115142822
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 85.65605163574219
         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: 28.252656936645508
           Y: -35.49672317504883
@@ -628,15 +738,15 @@ Visualization Manager:
         Near Clip Distance: 0.009999999776482582
         Pitch: 0.5653983950614929
         Target Frame: <Fixed Frame>
+        Value: Orbit (rviz_default_plugins)
         Yaw: 0.9104044437408447
-      - Class: rviz/Orbit
+      - Class: rviz_default_plugins/Orbit
         Distance: 60.1053581237793
         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: 30.61589241027832
           Y: 29.98663330078125
@@ -648,16 +758,17 @@ Visualization Manager:
         Near Clip Distance: 0.009999999776482582
         Pitch: 0.315398633480072
         Target Frame: <Fixed Frame>
+        Value: Orbit (rviz_default_plugins)
         Yaw: 5.788588047027588
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1376
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -666,6 +777,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2488
-  X: 72
+  Width: 1920
+  X: 0
   Y: 27
diff --git a/src/IMU_Processing.cpp b/src/IMU_Processing.cpp
index 217511e7..79855525 100755
--- a/src/IMU_Processing.cpp
+++ b/src/IMU_Processing.cpp
@@ -11,6 +11,7 @@ which is included as part of this source code package.
 */
 
 #include "IMU_Processing.h"
+#include <rcpputils/asserts.hpp>
 
 const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); }
 
@@ -29,7 +30,7 @@ ImuProcess::ImuProcess() : Eye3d(M3D::Identity()),
   acc_s_last = Zero3d;
   Lid_offset_to_IMU = Zero3d;
   Lid_rot_to_IMU = Eye3d;
-  last_imu.reset(new sensor_msgs::Imu());
+  last_imu.reset(new sensor_msgs::msg::Imu());
   cur_pcl_un_.reset(new PointCloudXYZI());
 }
 
@@ -37,14 +38,14 @@ ImuProcess::~ImuProcess() {}
 
 void ImuProcess::Reset()
 {
-  ROS_WARN("Reset ImuProcess");
+  RCLCPP_WARN(rclcpp::get_logger(""), "Reset ImuProcess");
   mean_acc = V3D(0, 0, -1.0);
   mean_gyr = V3D(0, 0, 0);
   angvel_last = Zero3d;
   imu_need_init = true;
   init_iter_num = 1;
   IMUpose.clear();
-  last_imu.reset(new sensor_msgs::Imu());
+  last_imu.reset(new sensor_msgs::msg::Imu());
   cur_pcl_un_.reset(new PointCloudXYZI());
 }
 
@@ -107,7 +108,7 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, in
 {
   /** 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_INFO(rclcpp::get_logger(""),"IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
   V3D cur_acc, cur_gyr;
 
   if (b_first_frame)
@@ -245,8 +246,8 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_
   // cout<<"meas.imu.size: "<<meas.imu.size()<<endl;
   auto v_imu = meas.imu;
   v_imu.push_front(last_imu);
-  const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
-  const double &imu_end_time = v_imu.back()->header.stamp.toSec();
+  const double &imu_beg_time = stamp2Sec(v_imu.front()->header.stamp);
+  const double &imu_end_time = stamp2Sec(v_imu.back()->header.stamp);
   const double prop_beg_time = last_prop_end_time;
   // printf("[ IMU ] undistort input size: %zu \n", lidar_meas.pcl_proc_cur->points.size());
   // printf("[ IMU ] IMU data sequence size: %zu \n", meas.imu.size());
@@ -307,7 +308,7 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_
   double tau;
   if (!imu_time_init)
   {
-    // imu_time = v_imu.front()->header.stamp.toSec() - first_lidar_time;
+    // imu_time = stamp2Sec(v_imu.front()->header.stamp) - first_lidar_time;
     // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75);
     tau = 1.0;
     imu_time_init = true;
@@ -315,11 +316,10 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_
   else
   {
     tau = state_inout.inv_expo_time;
-    // ROS_ERROR("tau: %.6f !!!!!!", tau);
+    // RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"tau: %.6f !!!!!!", tau);
   }
   // state_inout.cov(6, 6) = 0.01;
-
-  // ROS_ERROR("lidar_meas.lio_vio_flg");
+  // RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"lidar_meas.lio_vio_flg");
   // cout<<"lidar_meas.lio_vio_flg: "<<lidar_meas.lio_vio_flg<<endl;
   switch (lidar_meas.lio_vio_flg)
   {
@@ -331,7 +331,7 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_
       auto head = v_imu[i];
       auto tail = v_imu[i + 1];
 
-      if (tail->header.stamp.toSec() < prop_beg_time) continue;
+      if (stamp2Sec(tail->header.stamp) < prop_beg_time) continue;
 
       angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
           0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
@@ -346,30 +346,30 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_
       // cout<<"acc_avr: "<<acc_avr.transpose()<<endl;
 
       // #ifdef DEBUG_PRINT
-      fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
+      fout_imu << setw(10) << stamp2Sec(head->header.stamp) - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
       // #endif
 
-      // imu_time = head->header.stamp.toSec() - first_lidar_time;
+      // imu_time = stamp2Sec(head->header.stamp) - first_lidar_time;
 
       angvel_avr -= state_inout.bias_g;
       acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a;
 
-      if (head->header.stamp.toSec() < prop_beg_time)
+      if (stamp2Sec(head->header.stamp) < prop_beg_time)
       {
         // printf("00 \n");
-        dt = tail->header.stamp.toSec() - last_prop_end_time;
-        offs_t = tail->header.stamp.toSec() - prop_beg_time;
+        dt = stamp2Sec(tail->header.stamp) - last_prop_end_time;
+        offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time;
       }
       else if (i != v_imu.size() - 2)
       {
         // printf("11 \n");
-        dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
-        offs_t = tail->header.stamp.toSec() - prop_beg_time;
+        dt = stamp2Sec(tail->header.stamp) - stamp2Sec(head->header.stamp);
+        offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time;
       }
       else
       {
         // printf("22 \n");
-        dt = prop_end_time - head->header.stamp.toSec();
+        dt = prop_end_time - stamp2Sec(head->header.stamp);
         offs_t = prop_end_time - prop_beg_time;
       }
 
@@ -426,8 +426,8 @@ void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_
       angvel_last = angvel_avr;
       acc_s_last = acc_imu;
 
-      // cout<<setw(20)<<"offset_t: "<<offs_t<<"tail->header.stamp.toSec():
-      // "<<tail->header.stamp.toSec()<<endl; printf("[ LIO Propagation ]
+      // cout<<setw(20)<<"offset_t: "<<offs_t<<"stamp2Sec(tail->header.stamp):
+      // "<<stamp2Sec(tail->header.stamp)<<endl; printf("[ LIO Propagation ]
       // offs_t: %lf \n", offs_t);
       IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu));
     }
@@ -546,7 +546,7 @@ void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, Poin
 {
   double t1, t2, t3;
   t1 = omp_get_wtime();
-  ROS_ASSERT(lidar_meas.lidar != nullptr);
+  rcpputils::assert_true(lidar_meas.lidar != nullptr);
   if (!imu_en)
   {
     Forward_without_imu(lidar_meas, stat, *cur_pcl_un_);
@@ -572,11 +572,11 @@ void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, Poin
     {
       // cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
       imu_need_init = false;
-      ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: "
+      RCLCPP_INFO(rclcpp::get_logger(""), "IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: "
                "%.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f \n",
                stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm(), cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1],
                cov_gyr[2]);
-      ROS_INFO("IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: "
+      RCLCPP_INFO(rclcpp::get_logger(""), "IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: "
                "%.8f %.8f %.8f",
                cov_bias_acc[0], cov_bias_acc[1], cov_bias_acc[2], cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2]);
       fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out);
diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp
index 8002cfde..d8bc2e6c 100755
--- a/src/LIVMapper.cpp
+++ b/src/LIVMapper.cpp
@@ -11,22 +11,25 @@ which is included as part of this source code package.
 */
 
 #include "LIVMapper.h"
+#include <vikit/camera_loader.h>
 
-LIVMapper::LIVMapper(ros::NodeHandle &nh)
-    : extT(0, 0, 0),
+using namespace Sophus;
+LIVMapper::LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name, const rclcpp::NodeOptions & options)
+    : node(std::make_shared<rclcpp::Node>(node_name, options)),
+      extT(0, 0, 0),
       extR(M3D::Identity())
 {
   extrinT.assign(3, 0.0);
   extrinR.assign(9, 0.0);
   cameraextrinT.assign(3, 0.0);
   cameraextrinR.assign(9, 0.0);
-
+  
   p_pre.reset(new Preprocess());
   p_imu.reset(new ImuProcess());
 
-  readParameters(nh);
+  readParameters(this->node);
   VoxelMapConfig voxel_config;
-  loadVoxelConfig(nh, voxel_config);
+  loadVoxelConfig(this->node, voxel_config);
 
   visual_sub_map.reset(new PointCloudXYZI());
   feats_undistort.reset(new PointCloudXYZI());
@@ -40,86 +43,164 @@ LIVMapper::LIVMapper(ros::NodeHandle &nh)
   vio_manager.reset(new VIOManager());
   root_dir = ROOT_DIR;
   initializeFiles();
-  initializeComponents();
-  path.header.stamp = ros::Time::now();
+  initializeComponents(this->node);          // initialize components errors
+  path.header.stamp = this->node->now();
   path.header.frame_id = "camera_init";
 }
 
 LIVMapper::~LIVMapper() {}
 
-void LIVMapper::readParameters(ros::NodeHandle &nh)
+void LIVMapper::readParameters(rclcpp::Node::SharedPtr &node)
 {
-  nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar");
-  nh.param<string>("common/imu_topic", imu_topic, "/livox/imu");
-  nh.param<bool>("common/ros_driver_bug_fix", ros_driver_fix_en, false);
-  nh.param<int>("common/img_en", img_en, 1);
-  nh.param<int>("common/lidar_en", lidar_en, 1);
-  nh.param<string>("common/img_topic", img_topic, "/left_camera/image");
-
-  nh.param<bool>("vio/normal_en", normal_en, true);
-  nh.param<bool>("vio/inverse_composition_en", inverse_composition_en, false);
-  nh.param<int>("vio/max_iterations", max_iterations, 5);
-  nh.param<double>("vio/img_point_cov", IMG_POINT_COV, 100);
-  nh.param<bool>("vio/raycast_en", raycast_en, false);
-  nh.param<bool>("vio/exposure_estimate_en", exposure_estimate_en, true);
-  nh.param<double>("vio/inv_expo_cov", inv_expo_cov, 0.2);
-  nh.param<int>("vio/grid_size", grid_size, 5);
-  nh.param<int>("vio/grid_n_height", grid_n_height, 17);
-  nh.param<int>("vio/patch_pyrimid_level", patch_pyrimid_level, 3);
-  nh.param<int>("vio/patch_size", patch_size, 8);
-  nh.param<double>("vio/outlier_threshold", outlier_threshold, 1000);
-
-  nh.param<double>("time_offset/exposure_time_init", exposure_time_init, 0.0);
-  nh.param<double>("time_offset/img_time_offset", img_time_offset, 0.0);
-  nh.param<double>("time_offset/imu_time_offset", imu_time_offset, 0.0);
-  nh.param<bool>("uav/imu_rate_odom", imu_prop_enable, false);
-  nh.param<bool>("uav/gravity_align_en", gravity_align_en, false);
-
-  nh.param<string>("evo/seq_name", seq_name, "01");
-  nh.param<bool>("evo/pose_output_en", pose_output_en, false);
-  nh.param<double>("imu/gyr_cov", gyr_cov, 1.0);
-  nh.param<double>("imu/acc_cov", acc_cov, 1.0);
-  nh.param<int>("imu/imu_int_frame", imu_int_frame, 3);
-  nh.param<bool>("imu/imu_en", imu_en, false);
-  nh.param<bool>("imu/gravity_est_en", gravity_est_en, true);
-  nh.param<bool>("imu/ba_bg_est_en", ba_bg_est_en, true);
-
-  nh.param<double>("preprocess/blind", p_pre->blind, 0.01);
-  nh.param<double>("preprocess/filter_size_surf", filter_size_surf_min, 0.5);
-  nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA);
-  nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 6);
-  nh.param<int>("preprocess/point_filter_num", p_pre->point_filter_num, 3);
-  nh.param<bool>("preprocess/feature_extract_enabled", p_pre->feature_enabled, false);
-
-  nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
-  nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
-  nh.param<bool>("pcd_save/colmap_output_en", colmap_output_en, false);
-  nh.param<double>("pcd_save/filter_size_pcd", filter_size_pcd, 0.5);
-  nh.param<vector<double>>("extrin_calib/extrinsic_T", extrinT, vector<double>());
-  nh.param<vector<double>>("extrin_calib/extrinsic_R", extrinR, vector<double>());
-  nh.param<vector<double>>("extrin_calib/Pcl", cameraextrinT, vector<double>());
-  nh.param<vector<double>>("extrin_calib/Rcl", cameraextrinR, vector<double>());
-  nh.param<double>("debug/plot_time", plot_time, -10);
-  nh.param<int>("debug/frame_cnt", frame_cnt, 6);
-
-  nh.param<double>("publish/blind_rgb_points", blind_rgb_points, 0.01);
-  nh.param<int>("publish/pub_scan_num", pub_scan_num, 1);
-  nh.param<bool>("publish/pub_effect_point_en", pub_effect_point_en, false);
-  nh.param<bool>("publish/dense_map_en", dense_map_en, false);
+  // declare parameters
+  auto try_declare = [node]<typename ParameterT>(const std::string & name,
+    const ParameterT & default_value)
+  {
+    if (!node->has_parameter(name))
+    {
+      return node->declare_parameter<ParameterT>(name, default_value);
+    }
+    else
+    {
+      return node->get_parameter(name).get_value<ParameterT>();
+    }
+  };
+
+  // declare parameter
+  try_declare.template operator()<std::string>("common.lid_topic", "/livox/lidar");
+  try_declare.template operator()<std::string>("common.imu_topic", "/livox/imu");
+  try_declare.template operator()<bool>("common.ros_driver_bug_fix", false);
+  try_declare.template operator()<int>("common.img_en", 1);
+  try_declare.template operator()<int>("common.lidar_en", 1);
+  try_declare.template operator()<std::string>("common.img_topic", "/left_camera/image");
+
+  try_declare.template operator()<bool>("vio.normal_en", true);
+  try_declare.template operator()<bool>("vio.inverse_composition_en", false);
+  try_declare.template operator()<int>("vio.max_iterations", 5);
+  try_declare.template operator()<int>("vio.img_point_cov", 100);
+  try_declare.template operator()<bool>("vio.raycast_en", false);
+  try_declare.template operator()<bool>("vio.exposure_estimate_en", true);
+  try_declare.template operator()<double>("vio.inv_expo_cov", 0.1);
+  try_declare.template operator()<int>("vio.grid_size", 5);
+  try_declare.template operator()<int>("vio.grid_n_height", 17);
+  try_declare.template operator()<int>("vio.patch_pyrimid_level", 4);
+  try_declare.template operator()<int>("vio.patch_size", 8);
+  try_declare.template operator()<int>("vio.outlier_threshold", 100);
+  try_declare.template operator()<double>("time_offset.exposure_time_init", 0.0);
+  try_declare.template operator()<double>("time_offset.img_time_offset", 0.0);
+  try_declare.template operator()<bool>("uav.imu_rate_odom", false);
+  try_declare.template operator()<bool>("uav.gravity_align_en", false);
+
+  try_declare.template operator()<std::string>("evo.seq_name", "01");
+  try_declare.template operator()<bool>("evo.pose_output_en", false);
+  try_declare.template operator()<double>("imu.gyr_cov", 1.0);
+  try_declare.template operator()<double>("imu.acc_cov", 1.0);
+  try_declare.template operator()<int>("imu.imu_int_frame", 30);
+  try_declare.template operator()<bool>("imu.imu_en", true);
+  try_declare.template operator()<bool>("imu.gravity_est_en", true);
+  try_declare.template operator()<bool>("imu.ba_bg_est_en", true);
+
+  try_declare.template operator()<double>("preprocess.blind", 0.01);
+  try_declare.template operator()<double>("preprocess.filter_size_surf", 0.5);
+  try_declare.template operator()<int>("preprocess.lidar_type", AVIA);
+  try_declare.template operator()<int>("preprocess.scan_line",6);
+  try_declare.template operator()<int>("preprocess.point_filter_num", 3);
+  try_declare.template operator()<int>("preprocess.scan_rate", 10);
+  try_declare.template operator()<bool>("preprocess.feature_extract_enabled", false);
+
+  try_declare.template operator()<int>("pcd_save.interval", -1);
+  try_declare.template operator()<bool>("pcd_save.pcd_save_en", false);
+  try_declare.template operator()<bool>("pcd_save.colmap_output_en", false);
+  try_declare.template operator()<double>("pcd_save.filter_size_pcd", 0.5);
+  try_declare.template operator()<vector<double>>("extrin_calib.extrinsic_T", vector<double>{});
+  try_declare.template operator()<vector<double>>("extrin_calib.extrinsic_R", vector<double>{});
+  try_declare.template operator()<vector<double>>("extrin_calib.Pcl", vector<double>{});
+  try_declare.template operator()<vector<double>>("extrin_calib.Rcl", vector<double>{});
+  try_declare.template operator()<double>("debug.plot_time", -10);
+  try_declare.template operator()<int>("debug.frame_cnt", 6);
+
+  try_declare.template operator()<double>("publish.blind_rgb_points", 0.01);
+  try_declare.template operator()<int>("publish.pub_scan_num", 1);
+  try_declare.template operator()<bool>("publish.pub_effect_point_en", false);
+  try_declare.template operator()<bool>("publish.dense_map_en", false);
+
+  // get parameter
+  this->node->get_parameter("common.lid_topic", lid_topic);
+  this->node->get_parameter("common.imu_topic", imu_topic);
+  this->node->get_parameter("common.ros_driver_bug_fix", ros_driver_fix_en);
+  this->node->get_parameter("common.img_en", img_en);
+  this->node->get_parameter("common.lidar_en", lidar_en);
+  this->node->get_parameter("common.img_topic", img_topic);
+
+  this->node->get_parameter("vio.normal_en", normal_en);
+  this->node->get_parameter("vio.inverse_composition_en", inverse_composition_en);
+  this->node->get_parameter("vio.max_iterations", max_iterations);
+  this->node->get_parameter("vio.img_point_cov", IMG_POINT_COV);
+  this->node->get_parameter("vio.raycast_en", raycast_en);
+  this->node->get_parameter("vio.exposure_estimate_en", exposure_estimate_en);
+  this->node->get_parameter("vio.inv_expo_cov", inv_expo_cov);
+  this->node->get_parameter("vio.grid_size", grid_size);
+  this->node->get_parameter("vio.grid_n_height", grid_n_height);
+  this->node->get_parameter("vio.patch_pyrimid_level", patch_pyrimid_level);
+  this->node->get_parameter("vio.patch_size", patch_size);
+  this->node->get_parameter("vio.outlier_threshold", outlier_threshold);
+  this->node->get_parameter("time_offset.exposure_time_init", exposure_time_init);
+  this->node->get_parameter("time_offset.img_time_offset", img_time_offset);
+  this->node->get_parameter("uav.imu_rate_odom", imu_prop_enable);
+  this->node->get_parameter("uav.gravity_align_en", gravity_align_en);
+
+  this->node->get_parameter("evo.seq_name", seq_name);
+  this->node->get_parameter("evo.pose_output_en", pose_output_en);
+  this->node->get_parameter("imu.gyr_cov", gyr_cov);
+  this->node->get_parameter("imu.acc_cov", acc_cov);
+  this->node->get_parameter("imu.imu_int_frame", imu_int_frame);
+  this->node->get_parameter("imu.imu_en", imu_en);
+  this->node->get_parameter("imu.gravity_est_en", gravity_est_en);
+  this->node->get_parameter("imu.ba_bg_est_en", ba_bg_est_en);
+
+  this->node->get_parameter("preprocess.blind", p_pre->blind);
+  this->node->get_parameter("preprocess.filter_size_surf", filter_size_surf_min);
+  this->node->get_parameter("preprocess.lidar_type", p_pre->lidar_type);
+  this->node->get_parameter("preprocess.scan_line", p_pre->N_SCANS);
+  this->node->get_parameter("preprocess.scan_rate", p_pre->SCAN_RATE);
+  this->node->get_parameter("preprocess.point_filter_num", p_pre->point_filter_num);
+  this->node->get_parameter("preprocess.feature_extract_enabled", p_pre->feature_enabled);
+
+  this->node->get_parameter("pcd_save.interval", pcd_save_interval);
+  this->node->get_parameter("pcd_save.pcd_save_en", pcd_save_en);
+  this->node->get_parameter("pcd_save.colmap_output_en", colmap_output_en);
+  this->node->get_parameter("pcd_save.filter_size_pcd", filter_size_pcd);
+  this->node->get_parameter("extrin_calib.extrinsic_T", extrinT);
+  this->node->get_parameter("extrin_calib.extrinsic_R", extrinR);
+  this->node->get_parameter("extrin_calib.Pcl", cameraextrinT);
+  this->node->get_parameter("extrin_calib.Rcl", cameraextrinR);
+  this->node->get_parameter("debug.plot_time", plot_time);
+  this->node->get_parameter("debug.frame_cnt", frame_cnt);
+
+  this->node->get_parameter("publish.blind_rgb_points", blind_rgb_points);
+  this->node->get_parameter("publish.pub_scan_num", pub_scan_num);
+  this->node->get_parameter("publish.pub_effect_point_en", pub_effect_point_en);
+  this->node->get_parameter("publish.dense_map_en", dense_map_en);
 
   p_pre->blind_sqr = p_pre->blind * p_pre->blind;
 }
 
-void LIVMapper::initializeComponents() 
+void LIVMapper::initializeComponents(rclcpp::Node::SharedPtr &node) 
 {
   downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
+  
+  // extrinT.assign({0.04165, 0.02326, -0.0284});
+  // extrinR.assign({1, 0, 0, 0, 1, 0, 0, 0, 1});
+  // cameraextrinT.assign({0.0194384, 0.104689,-0.0251952});
+  // cameraextrinR.assign({0.00610193,-0.999863,-0.0154172,-0.00615449,0.0153796,-0.999863,0.999962,0.00619598,-0.0060598});
+
   extT << VEC_FROM_ARRAY(extrinT);
   extR << MAT_FROM_ARRAY(extrinR);
 
   voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT);
   voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR);
 
-  if (!vk::camera_loader::loadFromRosNs("laserMapping", vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified.");
+  if (!vk::camera_loader::loadFromRosNs(this->node, "camera", vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified.");
 
   vio_manager->grid_size = grid_size;
   vio_manager->patch_size = patch_size;
@@ -182,31 +263,34 @@ void LIVMapper::initializeFiles()
   fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out);
 }
 
-void LIVMapper::initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it) 
+void LIVMapper::initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &node, image_transport::ImageTransport &it_)
 {
-  sub_pcl = p_pre->lidar_type == AVIA ? 
-            nh.subscribe(lid_topic, 200000, &LIVMapper::livox_pcl_cbk, this): 
-            nh.subscribe(lid_topic, 200000, &LIVMapper::standard_pcl_cbk, this);
-  sub_imu = nh.subscribe(imu_topic, 200000, &LIVMapper::imu_cbk, this);
-  sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::img_cbk, this);
+  image_transport::ImageTransport it(this->node);
+  if (p_pre->lidar_type == AVIA) {
+    sub_pcl = this->node->create_subscription<livox_ros_driver2::msg::CustomMsg>(lid_topic, 200000, std::bind(&LIVMapper::livox_pcl_cbk, this, std::placeholders::_1));
+  } else {
+    sub_pcl = this->node->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, 200000, std::bind(&LIVMapper::standard_pcl_cbk, this, std::placeholders::_1));
+  }
+  sub_imu = this->node->create_subscription<sensor_msgs::msg::Imu>(imu_topic, 200000, std::bind(&LIVMapper::imu_cbk, this, std::placeholders::_1));
+  sub_img = this->node->create_subscription<sensor_msgs::msg::Image>(img_topic, 200000, std::bind(&LIVMapper::img_cbk, this, std::placeholders::_1));
   
-  pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100);
-  pubNormal = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 100);
-  pubSubVisualMap = nh.advertise<sensor_msgs::PointCloud2>("/cloud_visual_sub_map_before", 100);
-  pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>("/cloud_effected", 100);
-  pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/Laser_map", 100);
-  pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 10);
-  pubPath = nh.advertise<nav_msgs::Path>("/path", 10);
-  plane_pub = nh.advertise<visualization_msgs::Marker>("/planner_normal", 1);
-  voxel_pub = nh.advertise<visualization_msgs::MarkerArray>("/voxels", 1);
-  pubLaserCloudDyn = nh.advertise<sensor_msgs::PointCloud2>("/dyn_obj", 100);
-  pubLaserCloudDynRmed = nh.advertise<sensor_msgs::PointCloud2>("/dyn_obj_removed", 100);
-  pubLaserCloudDynDbg = nh.advertise<sensor_msgs::PointCloud2>("/dyn_obj_dbg_hist", 100);
-  mavros_pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
+  pubLaserCloudFullRes = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered", 100);
+  pubNormal = this->node->create_publisher<visualization_msgs::msg::MarkerArray>("/visualization_marker", 100);
+  pubSubVisualMap = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_visual_sub_map_before", 100);
+  pubLaserCloudEffect = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_effected", 100);
+  pubLaserCloudMap = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 100);
+  pubOdomAftMapped = this->node->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 10);
+  pubPath = this->node->create_publisher<nav_msgs::msg::Path>("/path", 10);
+  plane_pub = this->node->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1);
+  voxel_pub = this->node->create_publisher<visualization_msgs::msg::MarkerArray>("/voxels", 1);
+  pubLaserCloudDyn = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj", 100);
+  pubLaserCloudDynRmed = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj_removed", 100);
+  pubLaserCloudDynDbg = this->node->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj_dbg_hist", 100);
+  mavros_pose_publisher = this->node->create_publisher<geometry_msgs::msg::PoseStamped>("/mavros/vision_pose/pose", 10);
   pubImage = it.advertise("/rgb_img", 1);
-  pubImuPropOdom = nh.advertise<nav_msgs::Odometry>("/LIVO2/imu_propagate", 10000);
-  imu_prop_timer = nh.createTimer(ros::Duration(0.004), &LIVMapper::imu_prop_callback, this);
-  voxelmap_manager->voxel_map_pub_= nh.advertise<visualization_msgs::MarkerArray>("/planes", 10000);
+  pubImuPropOdom = this->node->create_publisher<nav_msgs::msg::Odometry>("/LIVO2/imu_propagate", 10000);
+  imu_prop_timer = this->node->create_wall_timer(0.004s, std::bind(&LIVMapper::imu_prop_callback, this));
+  voxelmap_manager->voxel_map_pub_= this->node->create_publisher<visualization_msgs::msg::MarkerArray>("/planes", 10000);
 }
 
 void LIVMapper::handleFirstFrame() 
@@ -226,7 +310,7 @@ void LIVMapper::gravityAlignment()
   {
     std::cout << "Gravity Alignment Starts" << std::endl;
     V3D ez(0, 0, -1), gz(_state.gravity);
-    Quaterniond G_q_I0 = Quaterniond::FromTwoVectors(gz, ez);
+    Eigen::Quaterniond G_q_I0 = Eigen::Quaterniond::FromTwoVectors(gz, ez);
     M3D G_R_I0 = G_q_I0.toRotationMatrix();
 
     _state.pos_end = G_R_I0 * _state.pos_end;
@@ -383,12 +467,12 @@ void LIVMapper::handleLIO()
     {
       evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out);
       pos_opend = true;
-      if (!evoFile.is_open()) ROS_ERROR("open fail\n");
+      if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n");
     } 
     else 
     {
       evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app);
-      if (!evoFile.is_open()) ROS_ERROR("open fail\n");
+      if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n");
     }
     Eigen::Matrix4d outT;
     Eigen::Quaterniond q(_state.rot_end);
@@ -524,12 +608,12 @@ void LIVMapper::savePCD()
   }
 }
 
-void LIVMapper::run() 
+void LIVMapper::run(rclcpp::Node::SharedPtr &node) 
 {
-  ros::Rate rate(5000);
-  while (ros::ok()) 
+  rclcpp::Rate rate(5000);
+  while (rclcpp::ok()) 
   {
-    ros::spinOnce();
+    rclcpp::spin_some(this->node);
     if (!sync_packages(LidarMeasures)) 
     {
       rate.sleep();
@@ -566,11 +650,11 @@ void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D
   imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt;
 }
 
-void LIVMapper::imu_prop_callback(const ros::TimerEvent &e)
+void LIVMapper::imu_prop_callback()
 {
   if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { return; }
   mtx_buffer_imu_prop.lock();
-  new_imu = false; // 控制propagate频率和IMU频率一致
+  new_imu = false; // 控制 propagate 频率和 IMU 频率一致
   if (imu_prop_enable && !prop_imu_buffer.empty())
   {
     static double last_t_from_lidar_end_time = 0;
@@ -578,14 +662,14 @@ void LIVMapper::imu_prop_callback(const ros::TimerEvent &e)
     {
       imu_propagate = latest_ekf_state;
       // drop all useless imu pkg
-      while ((!prop_imu_buffer.empty() && prop_imu_buffer.front().header.stamp.toSec() < latest_ekf_time))
+      while ((!prop_imu_buffer.empty() && stamp2Sec(prop_imu_buffer.front().header.stamp) < latest_ekf_time))
       {
         prop_imu_buffer.pop_front();
       }
       last_t_from_lidar_end_time = 0;
       for (int i = 0; i < prop_imu_buffer.size(); i++)
       {
-        double t_from_lidar_end_time = prop_imu_buffer[i].header.stamp.toSec() - latest_ekf_time;
+        double t_from_lidar_end_time = stamp2Sec(prop_imu_buffer[i].header.stamp) - latest_ekf_time;
         double dt = t_from_lidar_end_time - last_t_from_lidar_end_time;
         // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl;
         V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z);
@@ -599,7 +683,7 @@ void LIVMapper::imu_prop_callback(const ros::TimerEvent &e)
     {
       V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z);
       V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z);
-      double t_from_lidar_end_time = newest_imu.header.stamp.toSec() - latest_ekf_time;
+      double t_from_lidar_end_time = stamp2Sec(newest_imu.header.stamp) - latest_ekf_time;
       double dt = t_from_lidar_end_time - last_t_from_lidar_end_time;
       prop_imu_once(imu_propagate, dt, acc_imu, omg_imu);
       last_t_from_lidar_end_time = t_from_lidar_end_time;
@@ -622,7 +706,7 @@ void LIVMapper::imu_prop_callback(const ros::TimerEvent &e)
     imu_prop_odom.twist.twist.linear.x = vel_i.x();
     imu_prop_odom.twist.twist.linear.y = vel_i.y();
     imu_prop_odom.twist.twist.linear.z = vel_i.z();
-    pubImuPropOdom.publish(imu_prop_odom);
+    pubImuPropOdom->publish(imu_prop_odom);
   }
   mtx_buffer_imu_prop.unlock();
 }
@@ -668,7 +752,7 @@ template <typename T> Matrix<T, 3, 1> LIVMapper::pointBodyToWorld(const Matrix<T
 {
   V3D p(pi[0], pi[1], pi[2]);
   p = (_state.rot_end * (extR * p + extT) + _state.pos_end);
-  Matrix<T, 3, 1> po(p[0], p[1], p[2]);
+  Eigen::Matrix<T, 3, 1> po(p[0], p[1], p[2]);
   return po;
 }
 
@@ -682,58 +766,58 @@ void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const
   po->intensity = pi->intensity;
 }
 
-void LIVMapper::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
+void LIVMapper::standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
 {
   if (!lidar_en) return;
   mtx_buffer.lock();
   // cout<<"got feature"<<endl;
-  if (msg->header.stamp.toSec() < last_timestamp_lidar)
+  if (stamp2Sec(msg->header.stamp) < last_timestamp_lidar)
   {
-    ROS_ERROR("lidar loop back, clear buffer");
+    RCLCPP_ERROR(this->node->get_logger(),"lidar loop back, clear buffer");
     lid_raw_data_buffer.clear();
   }
-  // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec());
+  // ROS_INFO("get point cloud at time: %.6f", stamp2Sec(msg->header.stamp));
   PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
   p_pre->process(msg, ptr);
   lid_raw_data_buffer.push_back(ptr);
-  lid_header_time_buffer.push_back(msg->header.stamp.toSec());
-  last_timestamp_lidar = msg->header.stamp.toSec();
+  lid_header_time_buffer.push_back(stamp2Sec(msg->header.stamp));
+  last_timestamp_lidar = stamp2Sec(msg->header.stamp);
 
   mtx_buffer.unlock();
   sig_buffer.notify_all();
 }
 
-void LIVMapper::livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in)
+void LIVMapper::livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in)
 {
   if (!lidar_en) return;
   mtx_buffer.lock();
-  livox_ros_driver::CustomMsg::Ptr msg(new livox_ros_driver::CustomMsg(*msg_in));
-  // if ((abs(msg->header.stamp.toSec() - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag)
+  livox_ros_driver2::msg::CustomMsg::SharedPtr msg(new livox_ros_driver2::msg::CustomMsg(*msg_in));
+  // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag)
   // {
-  //   ROS_WARN("lidar jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_lidar);
+  //   ROS_WARN("lidar jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_lidar);
   //   sync_jump_flag = true;
-  //   msg->header.stamp = ros::Time().fromSec(last_timestamp_lidar + 0.1);
+  //   msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_lidar + 0.1);
   // }
-  if (abs(last_timestamp_imu - msg->header.stamp.toSec()) > 1.0 && !imu_buffer.empty())
+  if (abs(last_timestamp_imu - stamp2Sec(msg->header.stamp)) > 1.0 && !imu_buffer.empty())
   {
-    double timediff_imu_wrt_lidar = last_timestamp_imu - msg->header.stamp.toSec();
-    printf("\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100);
+    double timediff_imu_wrt_lidar = last_timestamp_imu - stamp2Sec(msg->header.stamp);
+    RCLCPP_INFO(this->node->get_logger(), "\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100);
     // imu_time_offset = timediff_imu_wrt_lidar;
   }
 
-  double cur_head_time = msg->header.stamp.toSec();
-  ROS_INFO("Get LiDAR, its header time: %.6f", cur_head_time);
+  double cur_head_time = stamp2Sec(msg->header.stamp);
+  RCLCPP_INFO(this->node->get_logger(), "Get LiDAR, its header time: %.6f", cur_head_time);
   if (cur_head_time < last_timestamp_lidar)
   {
-    ROS_ERROR("lidar loop back, clear buffer");
+    RCLCPP_ERROR(this->node->get_logger(), "lidar loop back, clear buffer");
     lid_raw_data_buffer.clear();
   }
-  // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec());
+  RCLCPP_INFO(this->node->get_logger(), "get point cloud at time: %.6f", stamp2Sec(msg->header.stamp));
   PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
   p_pre->process(msg, ptr);
 
   if (!ptr || ptr->empty()) {
-    ROS_ERROR("Received an empty point cloud");
+    RCLCPP_ERROR(this->node->get_logger(), "Received an empty point cloud");
     mtx_buffer.unlock();
     return;
   }
@@ -746,23 +830,23 @@ void LIVMapper::livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_i
   sig_buffer.notify_all();
 }
 
-void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
+void LIVMapper::imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in)
 {
   if (!imu_en) return;
 
   if (last_timestamp_lidar < 0.0) return;
-  // ROS_INFO("get imu at time: %.6f", msg_in->header.stamp.toSec());
-  sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
-  msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - imu_time_offset);
-  double timestamp = msg->header.stamp.toSec();
+  RCLCPP_INFO(this->node->get_logger(), "get imu at time: %.6f", stamp2Sec(msg_in->header.stamp));
+  sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in));
+  msg->header.stamp = sec2Stamp(stamp2Sec(msg->header.stamp) - imu_time_offset);
+  double timestamp = stamp2Sec(msg->header.stamp);
 
   if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en))
   {
-    ROS_WARN("IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp);
+    RCLCPP_WARN(this->node->get_logger(), "IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp);
   }
 
   if (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp);
-  msg->header.stamp = ros::Time().fromSec(timestamp);
+  msg->header.stamp = sec2Stamp(timestamp);
 
   mtx_buffer.lock();
 
@@ -770,23 +854,22 @@ void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
   {
     mtx_buffer.unlock();
     sig_buffer.notify_all();
-    ROS_ERROR("imu loop back, offset: %lf \n", last_timestamp_imu - timestamp);
+    RCLCPP_ERROR(this->node->get_logger(), "imu loop back, offset: %lf \n", last_timestamp_imu - timestamp);
     return;
   }
 
-  // if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2)
-  // {
-
-  //   ROS_WARN("imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu);
-  //   mtx_buffer.unlock();
-  //   sig_buffer.notify_all();
-  //   return;
-  // }
+  if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2)
+  {
+    RCLCPP_WARN(this->node->get_logger(), "imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu);
+    mtx_buffer.unlock();
+    sig_buffer.notify_all();
+    return;
+  }
 
   last_timestamp_imu = timestamp;
 
   imu_buffer.push_back(msg);
-  // cout<<"got imu: "<<timestamp<<" imu size "<<imu_buffer.size()<<endl;
+  cout<<"got imu: "<<timestamp<<" imu size "<<imu_buffer.size()<<endl;
   mtx_buffer.unlock();
   if (imu_prop_enable)
   {
@@ -799,23 +882,23 @@ void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
   sig_buffer.notify_all();
 }
 
-cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
+cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg)
 {
   cv::Mat img;
-  img = cv_bridge::toCvCopy(img_msg, "bgr8")->image;
+  img = cv_bridge::toCvShare(img_msg, "bgr8")->image;
   return img;
 }
 
 // static int i = 0;
-void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in)
+void LIVMapper::img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in)
 {
   if (!img_en) return;
-  sensor_msgs::Image::Ptr msg(new sensor_msgs::Image(*msg_in));
-  // if ((abs(msg->header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag)
+  sensor_msgs::msg::Image::SharedPtr msg(new sensor_msgs::msg::Image(*msg_in));
+  // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag)
   // {
-  //   ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img);
+  //   RCLCPP_WARN(this->node->get_logger(), "img jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_img);
   //   sync_jump_flag = true;
-  //   msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1);
+  //   msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_img + 0.1);
   // }
 
   // Hiliti2022 40Hz
@@ -824,15 +907,15 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in)
   //   i++;
   //   if (i % 4 != 0) return;
   // }
-  // double msg_header_time =  msg->header.stamp.toSec();
-  double msg_header_time = msg->header.stamp.toSec() + img_time_offset;
+  // double msg_header_time =  stamp2Sec(msg->header.stamp);
+  double msg_header_time = stamp2Sec(msg->header.stamp) + img_time_offset;
   if (abs(msg_header_time - last_timestamp_img) < 0.001) return;
-  ROS_INFO("Get image, its header time: %.6f", msg_header_time);
+  RCLCPP_INFO(this->node->get_logger(), "Get image, its header time: %.6f", msg_header_time);
   if (last_timestamp_lidar < 0) return;
 
   if (msg_header_time < last_timestamp_img)
   {
-    ROS_ERROR("image loop back. \n");
+    RCLCPP_ERROR(this->node->get_logger(), "image loop back. \n");
     return;
   }
 
@@ -842,7 +925,7 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in)
 
   if (img_time_correct - last_timestamp_img < 0.02)
   {
-    ROS_WARN("Image need Jumps: %.6f", img_time_correct);
+    RCLCPP_WARN(this->node->get_logger(), "Image need Jumps: %.6f", img_time_correct);
     mtx_buffer.unlock();
     sig_buffer.notify_all();
     return;
@@ -900,7 +983,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
     mtx_buffer.lock();
     while (!imu_buffer.empty())
     {
-      if (imu_buffer.front()->header.stamp.toSec() > meas.lidar_frame_end_time) break;
+      if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.lidar_frame_end_time) break;
       m.imu.push_back(imu_buffer.front());
       imu_buffer.pop_front();
     }
@@ -941,19 +1024,19 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
       // meas.last_lio_update_time);
 
       double lid_newest_time = lid_header_time_buffer.back() + lid_raw_data_buffer.back()->points.back().curvature / double(1000);
-      double imu_newest_time = imu_buffer.back()->header.stamp.toSec();
+      double imu_newest_time = stamp2Sec(imu_buffer.back()->header.stamp);
 
       if (img_capture_time < meas.last_lio_update_time + 0.00001)
       {
         img_buffer.pop_front();
         img_time_buffer.pop_front();
-        ROS_ERROR("[ Data Cut ] Throw one image frame! \n");
+        RCLCPP_ERROR(this->node->get_logger(), "[ Data Cut ] Throw one image frame! \n");
         return false;
       }
 
       if (img_capture_time > lid_newest_time || img_capture_time > imu_newest_time)
       {
-        // ROS_ERROR("lost first camera frame");
+        // RCLCPP_ERROR(this->node->get_logger(), "lost first camera frame");
         // printf("img_capture_time, lid_newest_time, imu_newest_time: %lf , %lf
         // , %lf \n", img_capture_time, lid_newest_time, imu_newest_time);
         return false;
@@ -968,13 +1051,13 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
       mtx_buffer.lock();
       while (!imu_buffer.empty())
       {
-        if (imu_buffer.front()->header.stamp.toSec() > m.lio_time) break;
+        if (stamp2Sec(imu_buffer.front()->header.stamp) > m.lio_time) break;
 
-        if (imu_buffer.front()->header.stamp.toSec() > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front());
+        if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front());
 
         imu_buffer.pop_front();
         // printf("[ Data Cut ] imu time: %lf \n",
-        // imu_buffer.front()->header.stamp.toSec());
+        // stamp2Sec(imu_buffer.front()->header.stamp));
       }
       mtx_buffer.unlock();
       sig_buffer.notify_all();
@@ -1029,7 +1112,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
       meas.lio_vio_flg = VIO;
       // printf("[ Data Cut ] VIO \n");
       meas.measures.clear();
-      double imu_time = imu_buffer.front()->header.stamp.toSec();
+      double imu_time = stamp2Sec(imu_buffer.front()->header.stamp);
 
       struct MeasureGroup m;
       m.vio_time = img_capture_time;
@@ -1038,12 +1121,12 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
       mtx_buffer.lock();
       // while ((!imu_buffer.empty() && (imu_time < img_capture_time)))
       // {
-      //   imu_time = imu_buffer.front()->header.stamp.toSec();
+      //   imu_time = stamp2Sec(imu_buffer.front()->header.stamp);
       //   if (imu_time > img_capture_time) break;
       //   m.imu.push_back(imu_buffer.front());
       //   imu_buffer.pop_front();
       //   printf("[ Data Cut ] imu time: %lf \n",
-      //   imu_buffer.front()->header.stamp.toSec());
+      //   stamp2Sec(imu_buffer.front()->header.stamp));
       // }
       img_buffer.pop_front();
       img_time_buffer.pop_front();
@@ -1096,21 +1179,21 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
     return false;
   }
   }
-  ROS_ERROR("out sync");
+  RCLCPP_ERROR(this->node->get_logger(), "out sync");
 }
 
 void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager)
 {
   cv::Mat img_rgb = vio_manager->img_cp;
   cv_bridge::CvImage out_msg;
-  out_msg.header.stamp = ros::Time::now();
+  out_msg.header.stamp = this->node->get_clock()->now();
   // out_msg.header.frame_id = "camera_init";
   out_msg.encoding = sensor_msgs::image_encodings::BGR8;
   out_msg.image = img_rgb;
   pubImage.publish(out_msg.toImageMsg());
 }
 
-void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager)
+void LIVMapper::publish_frame_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager)
 {
   if (pcl_w_wait_pub->empty()) return;
   PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB());
@@ -1160,7 +1243,7 @@ void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes,
   }
 
   /*** Publish Frame ***/
-  sensor_msgs::PointCloud2 laserCloudmsg;
+  sensor_msgs::msg::PointCloud2 laserCloudmsg;
   if (img_en)
   {
     // cout << "RGB pointcloud size: " << laserCloudWorldRGB->size() << endl;
@@ -1170,9 +1253,9 @@ void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes,
   { 
     pcl::toROSMsg(*pcl_w_wait_pub, laserCloudmsg); 
   }
-  laserCloudmsg.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar);
+  laserCloudmsg.header.stamp = this->node->get_clock()->now(); //.fromSec(last_timestamp_lidar);
   laserCloudmsg.header.frame_id = "camera_init";
-  pubLaserCloudFullRes.publish(laserCloudmsg);
+  pubLaserCloudFullRes->publish(laserCloudmsg);
 
   /**************** save map ****************/
   /* 1. make sure you have enough memories
@@ -1222,7 +1305,7 @@ void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes,
   PointCloudXYZI().swap(*pcl_w_wait_pub);
 }
 
-void LIVMapper::publish_visual_sub_map(const ros::Publisher &pubSubVisualMap)
+void LIVMapper::publish_visual_sub_map(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubSubVisualMap)
 {
   PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map);
   int size = laserCloudFullRes->points.size(); if (size == 0) return;
@@ -1230,15 +1313,15 @@ void LIVMapper::publish_visual_sub_map(const ros::Publisher &pubSubVisualMap)
   *sub_pcl_visual_map_pub = *laserCloudFullRes;
   if (1)
   {
-    sensor_msgs::PointCloud2 laserCloudmsg;
+    sensor_msgs::msg::PointCloud2 laserCloudmsg;
     pcl::toROSMsg(*sub_pcl_visual_map_pub, laserCloudmsg);
-    laserCloudmsg.header.stamp = ros::Time::now();
+    laserCloudmsg.header.stamp = this->node->get_clock()->now();
     laserCloudmsg.header.frame_id = "camera_init";
-    pubSubVisualMap.publish(laserCloudmsg);
+    pubSubVisualMap->publish(laserCloudmsg);
   }
 }
 
-void LIVMapper::publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector<PointToPlane> &ptpl_list)
+void LIVMapper::publish_effect_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudEffect, const std::vector<PointToPlane> &ptpl_list)
 {
   int effect_feat_num = ptpl_list.size();
   PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1));
@@ -1248,11 +1331,11 @@ void LIVMapper::publish_effect_world(const ros::Publisher &pubLaserCloudEffect,
     laserCloudWorld->points[i].y = ptpl_list[i].point_w_[1];
     laserCloudWorld->points[i].z = ptpl_list[i].point_w_[2];
   }
-  sensor_msgs::PointCloud2 laserCloudFullRes3;
+  sensor_msgs::msg::PointCloud2 laserCloudFullRes3;
   pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
-  laserCloudFullRes3.header.stamp = ros::Time::now();
+  laserCloudFullRes3.header.stamp = this->node->get_clock()->now();
   laserCloudFullRes3.header.frame_id = "camera_init";
-  pubLaserCloudEffect.publish(laserCloudFullRes3);
+  pubLaserCloudEffect->publish(laserCloudFullRes3);
 }
 
 template <typename T> void LIVMapper::set_posestamp(T &out)
@@ -1266,39 +1349,40 @@ template <typename T> void LIVMapper::set_posestamp(T &out)
   out.orientation.w = geoQuat.w;
 }
 
-void LIVMapper::publish_odometry(const ros::Publisher &pubOdomAftMapped)
+void LIVMapper::publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr &pubOdomAftMapped)
 {
   odomAftMapped.header.frame_id = "camera_init";
   odomAftMapped.child_frame_id = "aft_mapped";
-  odomAftMapped.header.stamp = ros::Time::now(); //.ros::Time()fromSec(last_timestamp_lidar);
+  odomAftMapped.header.stamp = this->node->get_clock()->now(); //.ros::Time()fromSec(last_timestamp_lidar);
   set_posestamp(odomAftMapped.pose.pose);
 
-  static tf::TransformBroadcaster br;
-  tf::Transform transform;
-  tf::Quaternion q;
-  transform.setOrigin(tf::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2)));
+  static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
+  br = std::make_shared<tf2_ros::TransformBroadcaster>(this->node);
+  tf2::Transform transform;
+  tf2::Quaternion q;
+  transform.setOrigin(tf2::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2)));
   q.setW(geoQuat.w);
   q.setX(geoQuat.x);
   q.setY(geoQuat.y);
   q.setZ(geoQuat.z);
   transform.setRotation(q);
-  br.sendTransform( tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped") );
-  pubOdomAftMapped.publish(odomAftMapped);
+  br->sendTransform(geometry_msgs::msg::TransformStamped(createTransformStamped(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped")));
+  pubOdomAftMapped->publish(odomAftMapped);
 }
 
-void LIVMapper::publish_mavros(const ros::Publisher &mavros_pose_publisher)
+void LIVMapper::publish_mavros(const rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr &mavros_pose_publisher)
 {
-  msg_body_pose.header.stamp = ros::Time::now();
+  msg_body_pose.header.stamp = this->node->get_clock()->now();
   msg_body_pose.header.frame_id = "camera_init";
   set_posestamp(msg_body_pose.pose);
-  mavros_pose_publisher.publish(msg_body_pose);
+  mavros_pose_publisher->publish(msg_body_pose);
 }
 
-void LIVMapper::publish_path(const ros::Publisher pubPath)
+void LIVMapper::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 = this->node->get_clock()->now();
   msg_body_pose.header.frame_id = "camera_init";
   path.poses.push_back(msg_body_pose);
-  pubPath.publish(path);
-}
\ No newline at end of file
+  pubPath->publish(path);
+}
diff --git a/src/frame.cpp b/src/frame.cpp
index 2a0b1268..643dd10e 100644
--- a/src/frame.cpp
+++ b/src/frame.cpp
@@ -10,7 +10,7 @@ This file is subject to the terms and conditions outlined in the 'LICENSE' file,
 which is included as part of this source code package.
 */
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 #include "feature.h"
 #include "frame.h"
 #include "visual_point.h"
diff --git a/src/main.cpp b/src/main.cpp
index 135b3363..73c969e4 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -2,11 +2,16 @@
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "laserMapping");
-  ros::NodeHandle nh;
-  image_transport::ImageTransport it(nh);
-  LIVMapper mapper(nh); 
-  mapper.initializeSubscribersAndPublishers(nh, it);
-  mapper.run();
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  options.allow_undeclared_parameters(true);
+  options.automatically_declare_parameters_from_overrides(true);
+
+  rclcpp::Node::SharedPtr nh;
+  image_transport::ImageTransport it_(nh);
+  LIVMapper mapper(nh, "laserMapping", options);
+  mapper.initializeSubscribersAndPublishers(nh, it_);
+  mapper.run(nh);
+  rclcpp::shutdown();
   return 0;
-}
\ No newline at end of file
+}
diff --git a/src/preprocess.cpp b/src/preprocess.cpp
index 322776b0..e3a74fdb 100755
--- a/src/preprocess.cpp
+++ b/src/preprocess.cpp
@@ -19,6 +19,7 @@ Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), po
 {
   inf_bound = 10;
   N_SCANS = 6;
+  SCAN_RATE = 10;
   group_size = 8;
   disA = 0.01;
   disA = 0.1; // B?
@@ -48,16 +49,17 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
   feature_enabled = feat_en;
   lidar_type = lid_type;
   blind = bld;
+  blind_sqr = bld * bld;
   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::SharedPtr &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 (lidar_type)
   {
@@ -87,7 +89,7 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
   *pcl_out = pl_surf;
 }
 
-void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
+void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg)
 {
   pl_surf.clear();
   pl_corn.clear();
@@ -119,6 +121,8 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
         pl_full[i].z = msg->points[i].z;
         pl_full[i].intensity = msg->points[i].reflectivity;
         pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points
+        double range = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;
+        if (range < blind_sqr) continue;
 
         bool is_new = false;
         if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) ||
@@ -195,7 +199,7 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
   printf("[ Preprocess ] Output point number: %zu \n", pl_surf.points.size());
 }
 
-void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+void Preprocess::l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
 {
   pl_surf.clear();
   pl_corn.clear();
@@ -206,7 +210,7 @@ void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   pl_corn.reserve(plsize);
   pl_surf.reserve(plsize);
 
-  double time_stamp = msg->header.stamp.toSec();
+  double time_stamp = stamp2Sec(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++)
@@ -235,7 +239,7 @@ void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   // pub_func(pl_surf, pub_corn, msg->header.stamp);
 }
 
-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();
@@ -297,7 +301,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   }
   else
   {
-    double time_stamp = msg->header.stamp.toSec();
+    double time_stamp = stamp2Sec(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++)
@@ -335,7 +339,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
 
 #define MAX_LINE_NUM 64
 
-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();
@@ -344,15 +348,16 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   pcl::PointCloud<velodyne_ros::Point> pl_orig;
   pcl::fromROSMsg(*msg, pl_orig);
   int plsize = pl_orig.points.size();
+  if (plsize == 0) return;
   pl_surf.reserve(plsize);
 
   bool is_first[MAX_LINE_NUM];
   double yaw_fp[MAX_LINE_NUM] = {0};     // yaw of first scan point
-  double omega_l = 3.61;                 // scan angular velocity
+  double omega_l = 0.361 * SCAN_RATE;    // scan angular velocity
   float yaw_last[MAX_LINE_NUM] = {0.0};  // yaw of last scan point
   float time_last[MAX_LINE_NUM] = {0.0}; // last offset time
 
-  if (pl_orig.points[plsize - 1].t > 0) { given_offset_time = true; }
+  if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; }
   else
   {
     given_offset_time = false;
@@ -390,7 +395,7 @@ void Preprocess::velodyne_handler(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].t / 1000.0; // units: ms
+      added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms
 
       if (!given_offset_time)
       {
@@ -453,7 +458,7 @@ void Preprocess::velodyne_handler(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].t / 1000.0;
+      added_pt.curvature = pl_orig.points[i].time / 1000.0;
 
       if (!given_offset_time)
       {
@@ -502,7 +507,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   // pub_func(pl_surf, pub_corn, msg->header.stamp);
 }
 
-void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+void Preprocess::Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
 {
   pl_surf.clear();
 
@@ -553,7 +558,7 @@ void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg
   // cout << GREEN << "pl_surf.points[31000].timestamp: " << pl_surf.points[31000].curvature << RESET << endl;
 }
 
-void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+void Preprocess::xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
 {
   pl_surf.clear();
   pl_corn.clear();
@@ -933,11 +938,11 @@ 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;
@@ -1077,4 +1082,4 @@ bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &type
   if (d1 > edgea * d2 || (d1 - d2) > edgeb) { return false; }
 
   return true;
-}
\ No newline at end of file
+}
diff --git a/src/utils.cpp b/src/utils.cpp
new file mode 100644
index 00000000..1e453c82
--- /dev/null
+++ b/src/utils.cpp
@@ -0,0 +1,19 @@
+// utils.cpp
+#include <vector>
+#include <cstdint> // for int64_t
+#include <limits>  // for std::numeric_limits
+#include <stdexcept> // for std::out_of_range
+
+std::vector<int> convertToIntVectorSafe(const std::vector<int64_t>& int64_vector) {
+    std::vector<int> int_vector;
+    int_vector.reserve(int64_vector.size()); // 预留空间以提高效率
+
+    for (int64_t value : int64_vector) {
+        if (value < std::numeric_limits<int>::min() || value > std::numeric_limits<int>::max()) {
+            throw std::out_of_range("Value is out of range for int");
+        }
+        int_vector.push_back(static_cast<int>(value));
+    }
+
+    return int_vector;
+}
diff --git a/src/vio.cpp b/src/vio.cpp
index 2ff8fd0f..6e14fcfc 100755
--- a/src/vio.cpp
+++ b/src/vio.cpp
@@ -12,6 +12,7 @@ which is included as part of this source code package.
 
 #include "vio.h"
 
+using namespace Eigen;
 VIOManager::VIOManager()
 {
   // downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
@@ -250,12 +251,12 @@ void VIOManager::insertPointIntoVoxelMap(VisualPoint *pt_new)
 }
 
 void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, const V3D &normal_ref,
-                                                  const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref)
+                                                  const SE3<double> &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref)
 {
   // create homography matrix
   const V3D t = T_cur_ref.inverse().translation();
   const Eigen::Matrix3d H_cur_ref =
-      T_cur_ref.rotation_matrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose());
+      T_cur_ref.rotationMatrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose());
   // Compute affine warp matrix A_ref_cur using homography projection
   const int kHalfPatchSize = 4;
   V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref)));
@@ -273,7 +274,7 @@ void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, co
 }
 
 void VIOManager::getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref,
-                                        const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size,
+                                        const SE3<double> &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size,
                                         Matrix2d &A_cur_ref)
 {
   // Compute affine warp matrix A_ref_cur
@@ -457,7 +458,7 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector<pointWithVar> &
         if (pt == nullptr) continue;
         if (pt->obs_.size() == 0) continue;
 
-        V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt->normal_);
+        V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt->normal_);
         V3D dir(new_frame_->T_f_w_ * pt->pos_);
         if (dir[2] < 0) continue;
         // dir.normalize();
@@ -535,7 +536,7 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector<pointWithVar> &
             // sub_map_ray.push_back(pt); // cloud_visual_sub_map
             // add_sample = true;
 
-            V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt->normal_);
+            V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt->normal_);
             V3D dir(new_frame_->T_f_w_ * pt->pos_);
             if (dir[2] < 0) continue;
             dir.normalize();
@@ -698,7 +699,7 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector<pointWithVar> &
 
       if (normal_en)
       {
-        V3D norm_vec = (ref_ftr->T_f_w_.rotation_matrix() * pt->normal_).normalized();
+        V3D norm_vec = (ref_ftr->T_f_w_.rotationMatrix() * pt->normal_).normalized();
         
         V3D pf(ref_ftr->T_f_w_ * pt->pos_);
         // V3D pf_norm = pf.normalized();
@@ -864,7 +865,7 @@ void VIOManager::generateVisualMapPoints(cv::Mat img, vector<pointWithVar> &pg)
       pointWithVar pt_var = append_voxel_points[i];
       V3D pt = pt_var.point_w;
 
-      V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt_var.normal);
+      V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt_var.normal);
       V3D dir(new_frame_->T_f_w_ * pt);
       dir.normalize();
       double cos_theta = dir.dot(norm_vec);
@@ -935,7 +936,7 @@ void VIOManager::updateVisualMapPoints(cv::Mat img)
     SE3 pose_ref = last_feature->T_f_w_;
     SE3 delta_pose = pose_ref * pose_cur.inverse();
     double delta_p = delta_pose.translation().norm();
-    double delta_theta = (delta_pose.rotation_matrix().trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (delta_pose.rotation_matrix().trace() - 1));
+    double delta_theta = (delta_pose.rotationMatrix().trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (delta_pose.rotationMatrix().trace() - 1));
     if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true; // 0.5 || 0.3
 
     // Step 3: pixel distance
@@ -1046,7 +1047,7 @@ void VIOManager::updateReferencePatch(const unordered_map<VOXEL_LOCATION, VoxelO
       int count = 0;
 
       V3D pf = ref_patch_temp->T_f_w_ * pt->pos_;
-      V3D norm_vec = ref_patch_temp->T_f_w_.rotation_matrix() * pt->normal_;
+      V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_;
       pf.normalize();
       double cos_angle = pf.dot(norm_vec);
       // if(fabs(cos_angle) < 0.86) continue; // 20 degree
@@ -1136,7 +1137,7 @@ void VIOManager::projectPatchFromRefToCur(const unordered_map<VOXEL_LOCATION, Vo
       V2D pc(new_frame_->w2c(pt->pos_));
       V2D pc_prior(new_frame_->w2c_prior(pt->pos_));
 
-      V3D norm_vec(ref_ftr->T_f_w_.rotation_matrix() * pt->normal_);
+      V3D norm_vec(ref_ftr->T_f_w_.rotationMatrix() * pt->normal_);
       V3D pf(ref_ftr->T_f_w_ * pt->pos_);
 
       if (pf.dot(norm_vec) < 0) norm_vec = -norm_vec;
@@ -1350,7 +1351,7 @@ void VIOManager::precomputeReferencePatches(int level)
     double depth((pt->pos_ - pt->ref_patch->pos()).norm());
     V3D pf = pt->ref_patch->f_ * depth;
     V2D pc = pt->ref_patch->px_;
-    M3D R_ref_w = pt->ref_patch->T_f_w_.rotation_matrix();
+    M3D R_ref_w = pt->ref_patch->T_f_w_.rotationMatrix();
 
     computeProjectionJacobian(pf, Jdpi);
     p_w_hat << SKEW_SYM_MATRX(pt->pos_);
@@ -1693,7 +1694,7 @@ void VIOManager::updateFrameState(StatesGroup state)
   V3D Pwi(state.pos_end);
   Rcw = Rci * Rwi.transpose();
   Pcw = -Rci * Rwi.transpose() * Pwi + Pci;
-  new_frame_->T_f_w_ = SE3(Rcw, Pcw);
+  new_frame_->T_f_w_ = SE3(Eigen::Quaterniond(Rcw).normalized().toRotationMatrix(), Pcw);  // avoid R is not orthogonal
 }
 
 void VIOManager::plotTrackedPoints()
@@ -1771,7 +1772,7 @@ void VIOManager::dumpDataForColmap()
   pinhole_cam->undistortImage(img_rgb, img_rgb_undistort);
   cv::imwrite(image_path, img_rgb_undistort);
   
-  Eigen::Quaterniond q(new_frame_->T_f_w_.rotation_matrix());
+  Eigen::Quaterniond q(new_frame_->T_f_w_.rotationMatrix());
   Eigen::Vector3d t = new_frame_->T_f_w_.translation();
   fout_colmap << cnt << " "
             << std::fixed << std::setprecision(6)  // 保证浮点数精度为6位
diff --git a/src/voxel_map.cpp b/src/voxel_map.cpp
index 01923cca..bf773562 100644
--- a/src/voxel_map.cpp
+++ b/src/voxel_map.cpp
@@ -11,7 +11,7 @@ which is included as part of this source code package.
 */
 
 #include "voxel_map.h"
-
+using namespace Eigen;
 void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov)
 {
   if (pb[2] == 0) pb[2] = 0.0001;
@@ -33,23 +33,52 @@ void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_
   cov = direction * range_var * direction.transpose() + A * direction_var * A.transpose();
 }
 
-void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config)
+void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config)
 {
-  nh.param<bool>("publish/pub_plane_en", voxel_config.is_pub_plane_map_, false);
-  
-  nh.param<int>("lio/max_layer", voxel_config.max_layer_, 1);
-  nh.param<double>("lio/voxel_size", voxel_config.max_voxel_size_, 0.5);
-  nh.param<double>("lio/min_eigen_value", voxel_config.planner_threshold_, 0.01);
-  nh.param<double>("lio/sigma_num", voxel_config.sigma_num_, 3);
-  nh.param<double>("lio/beam_err", voxel_config.beam_err_, 0.02);
-  nh.param<double>("lio/dept_err", voxel_config.dept_err_, 0.05);
-  nh.param<vector<int>>("lio/layer_init_num", voxel_config.layer_init_num_, vector<int>{5,5,5,5,5});
-  nh.param<int>("lio/max_points_num", voxel_config.max_points_num_, 50);
-  nh.param<int>("lio/max_iterations", voxel_config.max_iterations_, 5);
-
-  nh.param<bool>("local_map/map_sliding_en", voxel_config.map_sliding_en, false);
-  nh.param<int>("local_map/half_map_size", voxel_config.half_map_size, 100);
-  nh.param<double>("local_map/sliding_thresh", voxel_config.sliding_thresh, 8);
+  auto try_declare = [node]<typename ParameterT>(const std::string & name,
+    const ParameterT & default_value)
+  {
+    if (!node->has_parameter(name))
+    {
+      return node->declare_parameter<ParameterT>(name, default_value);
+    }
+    else
+    {
+      return node->get_parameter(name).get_value<ParameterT>();
+    }
+  };
+
+  // declare parameter
+  try_declare.template operator()<bool>("publish.pub_plane_en", false);
+  try_declare.template operator()<int>("lio.max_layer", 1);
+  try_declare.template operator()<double>("lio.voxel_size", 0.5);
+  try_declare.template operator()<double>("lio.min_eigen_value", 0.01);
+  try_declare.template operator()<double>("lio.sigma_num", 3);
+  try_declare.template operator()<double>("lio.beam_err", 0.02);
+  try_declare.template operator()<double>("lio.dept_err", 0.05);
+
+  // Declaration of parameter of type std::vector<int> won't build, https://github.com/ros2/rclcpp/issues/1585  
+  try_declare.template operator()<vector<int64_t>>("lio.layer_init_num", std::vector<int64_t>{5,5,5,5,5}); 
+  try_declare.template operator()<int>("lio.max_points_num", 50);
+  try_declare.template operator()<int>("lio.min_iterations", 5);
+  try_declare.template operator()<bool>("local_map.map_sliding_en", false);
+  try_declare.template operator()<int>("local_map.half_map_size", 100);
+  try_declare.template operator()<double>("local_map.sliding_thresh", 8.0);
+
+  // get parameter
+  node->get_parameter("publish.pub_plane_en", voxel_config.is_pub_plane_map_);
+  node->get_parameter("lio.max_layer", voxel_config.max_layer_);
+  node->get_parameter("lio.voxel_size", voxel_config.max_voxel_size_);
+  node->get_parameter("lio.min_eigen_value", voxel_config.planner_threshold_);
+  node->get_parameter("lio.sigma_num", voxel_config.sigma_num_);
+  node->get_parameter("lio.beam_err", voxel_config.beam_err_);
+  node->get_parameter("lio.dept_err", voxel_config.dept_err_);
+  node->get_parameter("lio.layer_init_num", voxel_config.layer_init_num_);
+  node->get_parameter("lio.max_points_num", voxel_config.max_points_num_);
+  node->get_parameter("lio.min_iterations", voxel_config.max_iterations_);
+  node->get_parameter("local_map.map_sliding_en", voxel_config.map_sliding_en);
+  node->get_parameter("local_map.half_map_size", voxel_config.half_map_size);
+  node->get_parameter("local_map.sliding_thresh", voxel_config.sliding_thresh);
 }
 
 void VoxelOctoTree::init_plane(const std::vector<pointWithVar> &points, VoxelPlane *plane)
@@ -535,7 +564,7 @@ void VoxelMapManager::BuildVoxelMap()
   float planer_threshold = config_setting_.planner_threshold_;
   int max_layer = config_setting_.max_layer_;
   int max_points_num = config_setting_.max_points_num_;
-  std::vector<int> layer_init_num = config_setting_.layer_init_num_;
+  std::vector<int> layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_);
 
   std::vector<pointWithVar> input_points;
 
@@ -612,7 +641,7 @@ void VoxelMapManager::UpdateVoxelMap(const std::vector<pointWithVar> &input_poin
   float planer_threshold = config_setting_.planner_threshold_;
   int max_layer = config_setting_.max_layer_;
   int max_points_num = config_setting_.max_points_num_;
-  std::vector<int> layer_init_num = config_setting_.layer_init_num_;
+  std::vector<int> layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_);
   uint plsize = input_points.size();
   for (uint i = 0; i < plsize; i++)
   {
@@ -789,9 +818,9 @@ void VoxelMapManager::pubVoxelMap()
 {
   double max_trace = 0.25;
   double pow_num = 0.2;
-  ros::Rate loop(500);
+  rclcpp::Rate loop(500);
   float use_alpha = 0.8;
-  visualization_msgs::MarkerArray voxel_plane;
+  visualization_msgs::msg::MarkerArray voxel_plane;
   voxel_plane.markers.reserve(1000000);
   std::vector<VoxelPlane> pub_plane_list;
   for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++)
@@ -813,7 +842,7 @@ void VoxelMapManager::pubVoxelMap()
     else { alpha = 0; }
     pubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb);
   }
-  voxel_map_pub_.publish(voxel_plane);
+  voxel_map_pub_->publish(voxel_plane);
   loop.sleep();
 }
 
@@ -834,20 +863,20 @@ void VoxelMapManager::GetUpdatePlane(const VoxelOctoTree *current_octo, const in
   return;
 }
 
-void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane,
+void VoxelMapManager::pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane,
                                      const float alpha, const Eigen::Vector3d rgb)
 {
-  visualization_msgs::Marker plane;
+  visualization_msgs::msg::Marker plane;
   plane.header.frame_id = "camera_init";
-  plane.header.stamp = ros::Time();
+  plane.header.stamp = rclcpp::Time();
   plane.ns = plane_ns;
   plane.id = single_plane.id_;
-  plane.type = visualization_msgs::Marker::CYLINDER;
-  plane.action = visualization_msgs::Marker::ADD;
+  plane.type = visualization_msgs::msg::Marker::CYLINDER;
+  plane.action = visualization_msgs::msg::Marker::ADD;
   plane.pose.position.x = single_plane.center_[0];
   plane.pose.position.y = single_plane.center_[1];
   plane.pose.position.z = single_plane.center_[2];
-  geometry_msgs::Quaternion q;
+  geometry_msgs::msg::Quaternion q;
   CalcVectQuation(single_plane.x_normal_, single_plane.y_normal_, single_plane.normal_, q);
   plane.pose.orientation = q;
   plane.scale.x = 3 * sqrt(single_plane.max_eigen_value_);
@@ -857,12 +886,12 @@ void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub,
   plane.color.r = rgb(0);
   plane.color.g = rgb(1);
   plane.color.b = rgb(2);
-  plane.lifetime = ros::Duration();
+  plane.lifetime = rclcpp::Duration::from_seconds(0.01);
   plane_pub.markers.push_back(plane);
 }
 
 void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec,
-                                      geometry_msgs::Quaternion &q)
+                                      geometry_msgs::msg::Quaternion &q)
 {
   Eigen::Matrix3d rot;
   rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), z_vec(1), z_vec(2);
@@ -968,4 +997,4 @@ void VoxelMapManager::clearMemOutOfMap(const int& x_max,const int& x_min,const i
   }
   std::cout<<RED<<"[DEBUG]: Delete "<<delete_voxel_cout<<" root voxels"<<RESET<<"\n";
   // std::cout<<RED<<"[DEBUG]: Delete "<<delete_voxel_cout<<" voxels using "<<delete_time<<" s"<<RESET<<"\n";
-}
\ No newline at end of file
+}