diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7f51009e..eb7547db 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(fast_livo)
+cmake_minimum_required(VERSION 3.20)
+project(fastlivo2)
 
 set(CMAKE_BUILD_TYPE "Release")
 message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}")
@@ -11,136 +11,133 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 # Set common compile options
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions")
 
-# Specific settings for Debug build
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -g")
-
 # Detect CPU architecture
 message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}")
 
 # Specific settings for Release build
 if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(arm|aarch64|ARM|AARCH64)")
-  if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
-    # 64-bit ARM optimizations (e.g., RK3588 and Jetson Orin NX)
-    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -ffast-math")
-    message(STATUS "Using 64-bit ARM optimizations: -O3 -mcpu=native -mtune=native -ffast-math")
-  else()
-    # 32-bit ARM optimizations with NEON support
-    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math")
-    message(STATUS "Using 32-bit ARM optimizations: -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math")
-  endif()
+  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -ffast-math")
+  message(STATUS "Using ARM optimizations")
   add_definitions(-DARM_ARCH)
 else()
-  # x86-64 (Intel/AMD) optimizations
-  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native -mtune=native -funroll-loops") #-flto
-  message(STATUS "Using general x86 optimizations: -O3 -march=native -mtune=native -funroll-loops") 
+  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native -mtune=native -funroll-loops")
+  message(STATUS "Using x86 optimizations")
   add_definitions(-DX86_ARCH)
 endif()
 
-# Define project root directory
 add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
 
-# Detect CPU core count for potential multithreading optimization
-include(ProcessorCount)
-ProcessorCount(N)
-message(STATUS "Processor count: ${N}")
-
-# Set the number of cores for multithreading
-if(N GREATER 4)
-  math(EXPR PROC_NUM "4")
-  add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM})
-  message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}")
-elseif(N GREATER 1)
-  math(EXPR PROC_NUM "${N}")
-  add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM})
-  message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}")
-else()
-  add_definitions(-DMP_PROC_NUM=1)
-  message(STATUS "Single core detected. Multithreading disabled.")
-endif()
-
-# Check for OpenMP support
-find_package(OpenMP QUIET)
-if(OpenMP_CXX_FOUND)
-  message(STATUS "OpenMP found")
-  add_compile_options(${OpenMP_CXX_FLAGS})
-else()
-  message(STATUS "OpenMP not found, proceeding without it")
-endif()
-
-# Check for mimalloc support
-find_package(mimalloc QUIET)
-if(mimalloc_FOUND)
-  message(STATUS "mimalloc found")
-else()
-  message(STATUS "mimalloc not found, proceeding without it")
-endif()
+# Find required dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(tf2_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(vikit_common REQUIRED)
+find_package(livox_interfaces REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(OpenMP REQUIRED)
 
-# Find catkin and required dependencies
-find_package(catkin REQUIRED COMPONENTS
+set(dependencies
+  rclcpp
   geometry_msgs
   nav_msgs
   sensor_msgs
-  roscpp
-  rospy
   std_msgs
   pcl_ros
-  tf
-  livox_ros_driver
-  message_generation
-  eigen_conversions
-  vikit_common
-  vikit_ros
+  tf2_ros
   cv_bridge
   image_transport
+  Eigen3
+  PCL
+  OpenCV
+  Sophus
+  Boost
+  vikit_common
+  livox_interfaces
+  visualization_msgs
+  OpenMP
 )
 
-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
 include_directories(
-  ${catkin_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIR}
+  ${EIGEN3_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
   ${Sophus_INCLUDE_DIRS}
   include
 )
 
-# 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)
+# # set Sourcefiles
+# LIST(APPEND SOURCEFILES src/atan_camera.cpp
+#                         src/omni_camera.cpp
+#                         src/math_utils.cpp
+#                         src/vision.cpp
+#                         src/performance_monitor.cpp
+#                         src/robust_cost.cpp
+#                         src/user_input_thread.cpp
+#                         src/pinhole_camera.cpp
+#                         src/equidistant_camera.cpp
+#                         src/polynomial_camera.cpp
+#                         src/homography.cpp
+#                         src/img_align.cpp)
+
+set(Sophus_LIBRARIES libSophus.so)
+    
+# Link dependencies using ament
+add_library(laser_mapping
+  src/vio.cpp
+  src/frame.cpp
+  src/visual_point.cpp
+  src/voxel_map.cpp
+  src/preprocess.cpp
+  src/IMU_Processing.cpp
+  src/LIVMapper.cpp
+)
+
+ament_target_dependencies(laser_mapping
+  ${dependencies}
+)
 
-# Add the main executable
-add_executable(fastlivo_mapping src/main.cpp)
+add_executable(fastlivo_mapping
+  src/main.cpp
+)
 
-# Link libraries to the executable
 target_link_libraries(fastlivo_mapping
   laser_mapping
-  vio
-  lio
-  pre
-  imu_proc
-  ${catkin_LIBRARIES}
+  ${Sophus_LIBRARIES}
+  ${OpenCV_LIBS}
   ${PCL_LIBRARIES}
-  ${OpenCV_LIBRARIES}
   ${Sophus_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${OpenMP_CXX_LIBRARIES}
+)
+
+install(
+  TARGETS fastlivo_mapping
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install include directories
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch config rviz
+  DESTINATION share/${PROJECT_NAME}
 )
 
-# Link mimalloc if found
-if(mimalloc_FOUND)
-  target_link_libraries(fastlivo_mapping mimalloc)
-endif()
\ No newline at end of file
+ament_export_include_directories(include)
+ament_export_libraries(laser_mapping)
+ament_export_dependencies(${dependencies})
+ament_package()
\ No newline at end of file
diff --git a/Log/Colmap/sparse/0/cameras.txt b/Log/Colmap/sparse/0/cameras.txt
deleted file mode 100644
index 98f766cb..00000000
--- a/Log/Colmap/sparse/0/cameras.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-# Camera list with one line of data per camera:
-#   CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]
-1 PINHOLE 640 512 588.143715 588.107927 296.059369 254.543215
diff --git a/Log/Colmap/sparse/0/images.txt b/Log/Colmap/sparse/0/images.txt
deleted file mode 100644
index e69de29b..00000000
diff --git a/Log/Colmap/sparse/0/points3D.txt b/Log/Colmap/sparse/0/points3D.txt
deleted file mode 100644
index e69de29b..00000000
diff --git a/Log/PCD/.gitkeep b/Log/PCD/.gitkeep
deleted file mode 100644
index e69de29b..00000000
diff --git a/Log/guide.md b/Log/guide.md
deleted file mode 100755
index ebd4d114..00000000
--- a/Log/guide.md
+++ /dev/null
@@ -1 +0,0 @@
-Here are the saved debug records, which can be plotted using ./Log/plot.py.
diff --git a/Log/plot.py b/Log/plot.py
deleted file mode 100755
index b8dd1df6..00000000
--- a/Log/plot.py
+++ /dev/null
@@ -1,98 +0,0 @@
-# import matplotlib
-# matplotlib.use('Agg')
-import numpy as np
-import matplotlib.pyplot as plt
-
-fig, axs = plt.subplots(3,2)
-lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
-lab_out = ['', 'out-x', 'out-y', 'out-z']
-plot_ind = range(7,10)
-a_pre=np.loadtxt('mat_pre.txt')
-a_out=np.loadtxt('mat_out.txt')
-time_pre=a_pre[:,0]
-time_out=a_out[:,0]
-axs[0,0].set_title('Attitude')
-axs[1,0].set_title('Translation')
-axs[2,0].set_title('Velocity')
-axs[0,1].set_title('bg')
-axs[1,1].set_title('ba')
-axs[2,1].set_title('ExposureTime')
-for i in range(1,4):
-    for j in range(5):
-        axs[j%3, j//3].plot(time_pre, a_pre[:,i+j*3],'.-', label=lab_pre[i])
-        axs[j%3, j//3].plot(time_out, a_out[:,i+j*3],'.-', label=lab_out[i])
-        axs[j%3, j//3].legend()
-#for i in range(1,4):
-axs[2, 1].plot(time_pre, a_pre[:,1+5*3],'.-', label=lab_pre[0])
-axs[2, 1].plot(time_out, a_out[:,1+5*3],'.-', label=lab_pre[0])
-axs[2, 1].legend(['pre', 'out'])
-#axs[2, 1].legend(['pre', 'out'])
-
-for j in range(6):
-    # axs[j].set_xlim(386,389)
-    #if j==5:
-	#continue	 
-    axs[j%3, j//3].grid()
-    #axs[j%3, j/3].legend()
-plt.grid()
-
-#### Draw IMU data
-fig, axs = plt.subplots(2)
-imu=np.loadtxt('imu.txt')
-time=imu[:,0]
-axs[0].set_title('Gyroscope')
-axs[1].set_title('Accelerameter')
-lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
-lab_2 = ['acc-x', 'acc-y', 'acc-z']
-for i in range(3):
-    # if i==1:
-    axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
-    axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
-for i in range(2):
-    # axs[i].set_xlim(386,389)
-    axs[i].grid()
-    axs[i].legend()
-plt.grid()
-
-# #### Draw time calculation
-# plt.figure(3)
-# fig = plt.figure()
-# font1 = {'family' : 'Times New Roman',
-# 'weight' : 'normal',
-# 'size'   : 12,
-# }
-# c="red"
-# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
-# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
-# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
-# # n = a_out[:,1].size
-# # time_mean = a_out[:,1].mean()
-# # time_se   = a_out[:,1].std() / np.sqrt(n)
-# # time_err  = a_out[:,1] - time_mean
-# # feat_mean = a_out[:,2].mean()
-# # feat_err  = a_out[:,2] - feat_mean
-# # feat_se   = a_out[:,2].std() / np.sqrt(n)
-# ax1 = fig.add_subplot(111)
-# ax1.set_ylabel('Effective Feature Numbers',font1)
-# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
-# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
-# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
-# ax1.set_ylim([0, 3000])
-
-# ax2 = ax1.twinx()
-# ax2.spines['right'].set_color('red')
-# ax2.set_ylabel('Compute Time (ms)',font1)
-# ax2.yaxis.label.set_color('red')
-# ax2.tick_params(axis='y', colors='red')
-# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
-# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
-# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
-# ax2.set_xlim([0.5, 3.5])
-# ax2.set_ylim([0, 100])
-
-# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
-# # # print(time_se)
-# # # print(a_out3[:,2])
-# plt.grid()
-# plt.savefig("time.pdf", dpi=1200)
-plt.show()
diff --git a/config/avia.yaml b/config/avia.yaml
index 04c24bab..b961078d 100755
--- a/config/avia.yaml
+++ b/config/avia.yaml
@@ -1,85 +1,110 @@
-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
+/fastlivo2_node:
+  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., 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]
 
-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.0 # 78 100 156 #100 200 500 700 infinite
+      img_point_cov: 100.0 # 100 1000 
+      grid_size: 5
+      grid_n_height: 5
+      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
+      gravity_est_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
+      sigma_num: 3.0
+      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
+    time_offset:
+      imu_time_offset: 0.0
+      img_time_offset: 0.0
+      exposure_time_init: 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
+    uav:
+      imu_rate_odom: false
+      gravity_align_en: false
 
-evo:
-  seq_name: "CBD_Building_01"
-  pose_output_en: false
+    publish:
+      dense_map_en: true
+      pub_effect_point_en: true 
+      pub_plane_en: true
+      pub_scan_num: 1
+      blind_rgb_points: 0.0
 
-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.
+    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.
+
+    camera:
+      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
diff --git a/config/camera_pinhole.yaml b/config/camera_pinhole.yaml
deleted file mode 100644
index 83fd89e0..00000000
--- a/config/camera_pinhole.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
diff --git a/include/IMU_Processing.h b/include/fastlivo2/IMU_Processing.h
similarity index 89%
rename from include/IMU_Processing.h
rename to include/fastlivo2/IMU_Processing.h
index 4552b51c..d467c609 100644
--- a/include/IMU_Processing.h
+++ b/include/fastlivo2/IMU_Processing.h
@@ -13,12 +13,18 @@ which is included as part of this source code package.
 #ifndef IMU_PROCESSING_H
 #define IMU_PROCESSING_H
 
+#pragma once
+
+#include <fstream>
 #include <Eigen/Eigen>
-#include "common_lib.h"
 #include <condition_variable>
-#include <nav_msgs/Odometry.h>
 #include <utils/so3_math.h>
 
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
+#include "fastlivo2/common_lib.h"
+
 const bool time_list(PointType &x,
                      PointType &y); //{return (x.curvature < y.curvature);};
 
@@ -32,7 +38,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);
@@ -68,7 +74,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::SharedPtr last_imu;
   PointCloudXYZI::Ptr cur_pcl_un_;
   vector<Pose6D> IMUpose;
   M3D Lid_rot_to_IMU;
@@ -85,6 +91,8 @@ class ImuProcess
   bool gravity_est_en = true;
   bool ba_bg_est_en = true;
   bool exposure_estimate_en = true;
+
+  rclcpp::Logger logger;
 };
 typedef std::shared_ptr<ImuProcess> ImuProcessPtr;
 #endif
\ No newline at end of file
diff --git a/include/LIVMapper.h b/include/fastlivo2/LIVMapper.h
similarity index 52%
rename from include/LIVMapper.h
rename to include/fastlivo2/LIVMapper.h
index f211dfcb..fed54e88 100644
--- a/include/LIVMapper.h
+++ b/include/fastlivo2/LIVMapper.h
@@ -1,32 +1,36 @@
-/* 
-This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
-
-Developer: Chunran Zheng <zhengcr@connect.hku.hk>
-
-For commercial use, please contact me at <zhengcr@connect.hku.hk> or
-Prof. Fu Zhang at <fuzhang@hku.hk>.
-
-This file is subject to the terms and conditions outlined in the 'LICENSE' file,
-which is included as part of this source code package.
-*/
-
 #ifndef LIV_MAPPER_H
 #define LIV_MAPPER_H
 
-#include "IMU_Processing.h"
-#include "vio.h"
-#include "preprocess.h"
-#include <cv_bridge/cv_bridge.h>
-#include <image_transport/image_transport.h>
-#include <nav_msgs/Path.h>
-#include <vikit/camera_loader.h>
+#pragma once
 
-class LIVMapper
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <nav_msgs/msg/path.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <livox_interfaces/msg/custom_msg.hpp>
+
+// #include <vikit/camera_loader.h>
+#include <mutex>
+#include <condition_variable>
+
+#include "fastlivo2/IMU_Processing.h"
+#include "fastlivo2/vio.h"
+#include "fastlivo2/preprocess.h"
+
+class LIVMapper : public rclcpp::Node
 {
 public:
-  LIVMapper(ros::NodeHandle &nh);
+  LIVMapper(rclcpp::NodeOptions options);
   ~LIVMapper();
-  void initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it);
+
+  void initializeSubscribersAndPublishers(image_transport::ImageTransport &it);
   void initializeComponents();
   void initializeFiles();
   void run();
@@ -40,27 +44,28 @@ 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 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 standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void livox_pcl_cbk(const livox_interfaces::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();
+  void publish_frame_world();
+  void publish_visual_sub_map();
+  void publish_effect_world(const std::vector<PointToPlane> &ptpl_list);
+  void publish_odometry();
+  void publish_mavros();
+  void publish_path();
+  void readParameters();
+  
   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;
@@ -68,13 +73,11 @@ class LIVMapper
   SLAM_MODE slam_mode_;
   std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> voxel_map;
   
-  string root_dir;
-  string lid_topic, imu_topic, seq_name, img_topic;
+  std::string root_dir, lid_topic, imu_topic, seq_name, img_topic;
   V3D extT;
   M3D extR;
 
   int feats_down_size = 0, max_iterations = 0;
-
   double res_mean_last = 0.05;
   double gyr_cov = 0, acc_cov = 0, inv_expo_cov = 0;
   double blind_rgb_points = 0.0;
@@ -83,23 +86,22 @@ class LIVMapper
   double filter_size_pcd = 0;
   double _first_lidar_time = 0.0;
   double match_time = 0, solve_time = 0, solve_const_H_time = 0;
-
+  int pub_scan_num = 1;
+  
   bool lidar_map_inited = false, pcd_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false;
   int pcd_save_interval = -1, pcd_index = 0;
-  int pub_scan_num = 1;
-
+  
   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;
+  std::deque<sensor_msgs::msg::Imu> prop_imu_buffer;
+  sensor_msgs::msg::Imu newest_imu;
+  nav_msgs::msg::Odometry imu_prop_odom;
   double latest_ekf_time;
-  nav_msgs::Odometry imu_prop_odom;
-  ros::Publisher pubImuPropOdom;
   double imu_time_offset = 0.0;
-
+  
   bool gravity_align_en = false, gravity_align_finished = false;
-
+  
   bool sync_jump_flag = false;
 
   bool lidar_pushed = false, imu_en, gravity_est_en, flg_reset = false, ba_bg_est_en = true;
@@ -119,7 +121,7 @@ class LIVMapper
   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::SharedPtr> imu_buffer;
   deque<cv::Mat> img_buffer;
   deque<double> img_time_buffer;
   vector<pointWithVar> _pv_list;
@@ -147,34 +149,40 @@ 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;
-  image_transport::Publisher pubImage;
-  ros::Publisher mavros_pose_publisher;
-  ros::Timer imu_prop_timer;
+  
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubImuPropOdom;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
+  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<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDyn;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynDbg;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynRmed;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr mavros_pose_publisher;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_pub;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubNormal;
+
+  // rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pcl;
+  rclcpp::Subscription<livox_interfaces::msg::CustomMsg>::SharedPtr sub_pcl;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_img;
+  rclcpp::TimerBase::SharedPtr imu_prop_timer;
+
+  std::shared_ptr<image_transport::Publisher> pubImage;
+
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
 
   int frame_num = 0;
   double aver_time_consu = 0;
@@ -182,4 +190,5 @@ 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/fastlivo2/common_lib.h
similarity index 98%
rename from include/common_lib.h
rename to include/fastlivo2/common_lib.h
index f85267d2..c48277af 100755
--- a/include/common_lib.h
+++ b/include/fastlivo2/common_lib.h
@@ -13,13 +13,16 @@ which is included as part of this source code package.
 #ifndef COMMON_LIB_H
 #define COMMON_LIB_H
 
+#pragma once
+
 #include <utils/so3_math.h>
 #include <utils/types.h>
 #include <utils/color.h>
 #include <opencv2/opencv.hpp>
-#include <sensor_msgs/Imu.h>
 #include <sophus/se3.h>
-#include <tf/transform_broadcaster.h>
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <sensor_msgs/msg/imu.hpp>
 
 using namespace std;
 using namespace Eigen;
@@ -61,7 +64,7 @@ struct MeasureGroup
 {
   double vio_time;
   double lio_time;
-  deque<sensor_msgs::Imu::ConstPtr> imu;
+  deque<sensor_msgs::msg::Imu::SharedPtr> imu;
   cv::Mat img;
   MeasureGroup()
   {
diff --git a/include/feature.h b/include/fastlivo2/feature.h
similarity index 93%
rename from include/feature.h
rename to include/fastlivo2/feature.h
index 08a17f87..2553cb41 100644
--- a/include/feature.h
+++ b/include/fastlivo2/feature.h
@@ -13,7 +13,9 @@ which is included as part of this source code package.
 #ifndef LIVO_FEATURE_H_
 #define LIVO_FEATURE_H_
 
-#include "visual_point.h"
+#pragma once
+
+#include "fastlivo2/visual_point.h"
 
 // A salient image region that is tracked across frames.
 struct Feature
@@ -48,7 +50,7 @@ struct Feature
   
   ~Feature()
   {
-    // ROS_WARN("The feature %d has been destructed.", id_);
+    // RCLCPP_WARN(this->get_logger(), "The feature %d has been destructed.", id_);
     delete[] patch_;
   }
 };
diff --git a/include/frame.h b/include/fastlivo2/frame.h
similarity index 100%
rename from include/frame.h
rename to include/fastlivo2/frame.h
diff --git a/include/preprocess.h b/include/fastlivo2/preprocess.h
similarity index 74%
rename from include/preprocess.h
rename to include/fastlivo2/preprocess.h
index 649ed838..32e6eda0 100755
--- a/include/preprocess.h
+++ b/include/fastlivo2/preprocess.h
@@ -1,21 +1,15 @@
-/* 
-This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
-
-Developer: Chunran Zheng <zhengcr@connect.hku.hk>
-
-For commercial use, please contact me at <zhengcr@connect.hku.hk> or
-Prof. Fu Zhang at <fuzhang@hku.hk>.
-
-This file is subject to the terms and conditions outlined in the 'LICENSE' file,
-which is included as part of this source code package.
-*/
-
 #ifndef PREPROCESS_H_
 #define PREPROCESS_H_
 
-#include "common_lib.h"
-#include <livox_ros_driver/CustomMsg.h>
+#pragma once
+
+#include <omp.h>
+#include <rclcpp/rclcpp.hpp>
 #include <pcl_conversions/pcl_conversions.h>
+#include <livox_interfaces/msg/custom_msg.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include "fastlivo2/common_lib.h"
 
 using namespace std;
 
@@ -130,19 +124,16 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(Pandar128_ros::Point,
                                   (float, x, x)(float, y, y)(float, z, z)(float, timestamp, timestamp))
 /*****************/
 
-class Preprocess
+class Preprocess : public rclcpp::Node
 {
 public:
-  //   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
   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_interfaces::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
+  void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
   void set(bool feat_en, int lid_type, double bld, int pfilt_num);
 
-  // sensor_msgs::PointCloud2::ConstPtr pointcloud;
   PointCloudXYZI pl_full, pl_corn, pl_surf;
   PointCloudXYZI pl_buff[128]; // maximum 128 line lidar
   vector<orgtype> typess[128]; // maximum 128 line lidar
@@ -150,17 +141,16 @@ class Preprocess
   
   double blind, blind_sqr;
   bool feature_enabled, given_offset_time;
-  ros::Publisher pub_full, pub_surf, 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_interfaces::msg::CustomMsg::ConstSharedPtr &msg);
+  void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
+  void 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);
@@ -174,7 +164,13 @@ class Preprocess
   double edgea, edgeb;
   double smallp_intersect, smallp_ratio;
   double vx, vy, vz;
+
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_full;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_surf;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_corn;
+
 };
+
 typedef std::shared_ptr<Preprocess> PreprocessPtr;
 
-#endif // PREPROCESS_H_
\ No newline at end of file
+#endif // PREPROCESS_H_
diff --git a/include/vio.h b/include/fastlivo2/vio.h
similarity index 98%
rename from include/vio.h
rename to include/fastlivo2/vio.h
index d5600b1d..b2f7d05b 100755
--- a/include/vio.h
+++ b/include/fastlivo2/vio.h
@@ -13,8 +13,8 @@ which is included as part of this source code package.
 #ifndef VIO_H_
 #define VIO_H_
 
-#include "voxel_map.h"
-#include "feature.h"
+#pragma once
+
 #include <opencv2/imgproc/imgproc_c.h>
 #include <pcl/filters/voxel_grid.h>
 #include <set>
@@ -23,6 +23,9 @@ which is included as part of this source code package.
 #include <vikit/vision.h>
 #include <vikit/pinhole_camera.h>
 
+#include "fastlivo2/voxel_map.h"
+#include "fastlivo2/feature.h"
+
 struct SubSparseMap
 {
   vector<float> propa_errors;
diff --git a/include/visual_point.h b/include/fastlivo2/visual_point.h
similarity index 95%
rename from include/visual_point.h
rename to include/fastlivo2/visual_point.h
index 494cc726..d5f4bcbd 100644
--- a/include/visual_point.h
+++ b/include/fastlivo2/visual_point.h
@@ -13,9 +13,12 @@ which is included as part of this source code package.
 #ifndef LIVO_POINT_H_
 #define LIVO_POINT_H_
 
+#pragma once
+
 #include <boost/noncopyable.hpp>
-#include "common_lib.h"
-#include "frame.h"
+
+#include "fastlivo2/common_lib.h"
+#include "fastlivo2/frame.h"
 
 class Feature;
 
diff --git a/include/voxel_map.h b/include/fastlivo2/voxel_map.h
similarity index 86%
rename from include/voxel_map.h
rename to include/fastlivo2/voxel_map.h
index 21afaa34..a14182cf 100644
--- a/include/voxel_map.h
+++ b/include/fastlivo2/voxel_map.h
@@ -1,31 +1,23 @@
-/* 
-This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
-
-Developer: Chunran Zheng <zhengcr@connect.hku.hk>
-
-For commercial use, please contact me at <zhengcr@connect.hku.hk> or
-Prof. Fu Zhang at <fuzhang@hku.hk>.
-
-This file is subject to the terms and conditions outlined in the 'LICENSE' file,
-which is included as part of this source code package.
-*/
-
 #ifndef VOXEL_MAP_H_
 #define VOXEL_MAP_H_
 
-#include "common_lib.h"
+#pragma once
+
 #include <Eigen/Dense>
 #include <fstream>
 #include <math.h>
 #include <mutex>
 #include <omp.h>
 #include <pcl/common/io.h>
-#include <ros/ros.h>
 #include <thread>
 #include <unistd.h>
 #include <unordered_map>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include "fastlivo2/common_lib.h"
 
 #define VOXELMAP_HASH_P 116101
 #define VOXELMAP_MAX_N 10000000000
@@ -37,7 +29,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_;
@@ -137,7 +129,7 @@ class VoxelOctoTree
   int octo_state_; // 0 is end of tree, 1 is not
   VoxelOctoTree *leaves_[8];
   double voxel_center_[3]; // x, y, z
-  std::vector<int> layer_init_num_;
+  std::vector<int64_t> layer_init_num_;
   float quater_length_;
   float planer_threshold_;
   int points_size_threshold_;
@@ -182,7 +174,7 @@ class VoxelOctoTree
   VoxelOctoTree *Insert(const pointWithVar &pv);
 };
 
-void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config);
+void loadVoxelConfig(rclcpp::Node* node, VoxelMapConfig &voxel_config);
 
 class VoxelMapManager
 {
@@ -190,7 +182,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_;
@@ -206,9 +198,9 @@ class VoxelMapManager
   StatesGroup state_;
   V3D position_last_;
 
-  V3D last_slide_position = {0,0,0};
+  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,12 +240,12 @@ 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);
 };
 typedef std::shared_ptr<VoxelMapManager> VoxelMapManagerPtr;
 
-#endif // VOXEL_MAP_H_
\ No newline at end of file
+#endif // VOXEL_MAP_H_
diff --git a/include/vikit/camera_loader.h b/include/vikit/camera_loader.h
new file mode 100644
index 00000000..b63a0906
--- /dev/null
+++ b/include/vikit/camera_loader.h
@@ -0,0 +1,192 @@
+#ifndef VIKIT_CAMERA_LOADER_H_
+#define VIKIT_CAMERA_LOADER_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <rclcpp/rclcpp.hpp>
+
+#include <vikit/abstract_camera.h>
+#include <vikit/pinhole_camera.h>
+#include <vikit/atan_camera.h>
+#include <vikit/omni_camera.h>
+#include <vikit/equidistant_camera.h>
+#include <vikit/polynomial_camera.h>
+
+namespace vk {
+namespace camera_loader {
+
+/// Load a single camera from ROS 2 parameters
+bool loadFromRosNs(const rclcpp::Node* node, const std::string& ns, vk::AbstractCamera* &cam)
+{
+  std::string cam_model;
+  if (!node->get_parameter(ns + ".cam_model", cam_model)) {
+    RCLCPP_ERROR(node->get_logger(), "Camera model parameter not found: %s.cam_model", ns.c_str());
+    return false;
+  }
+
+  if (cam_model == "Ocam") {
+    std::string calib_file;
+    node->get_parameter(ns + ".cam_calib_file", calib_file);
+    cam = new vk::OmniCamera(calib_file);
+  }
+  else if (cam_model == "Pinhole") {
+    int cam_width = 640;
+    int cam_height = 480;
+    double scale = 1.0;
+    double cam_fx = 0.0;
+    double cam_fy = 0.0;
+    double cam_cx = 0.0;
+    double cam_cy = 0.0;
+    double cam_d0 = 0.0;
+    double cam_d1 = 0.0;
+    double cam_d2 = 0.0;
+    double cam_d3 = 0.0;
+
+    node->get_parameter(ns + ".cam_width", cam_width);
+    node->get_parameter(ns + ".cam_height", cam_height);
+    node->get_parameter(ns + ".scale", scale);
+    node->get_parameter(ns + ".cam_fx", cam_fx);
+    node->get_parameter(ns + ".cam_fy", cam_fy);
+    node->get_parameter(ns + ".cam_cx", cam_cx);
+    node->get_parameter(ns + ".cam_cy", cam_cy);
+    node->get_parameter(ns + ".cam_d0", cam_d0);
+    node->get_parameter(ns + ".cam_d1", cam_d1);
+    node->get_parameter(ns + ".cam_d2", cam_d2);
+    node->get_parameter(ns + ".cam_d3", cam_d3);
+
+    cam = new vk::PinholeCamera(
+      cam_width, cam_height, scale,
+      cam_fx, cam_fy, cam_cx, cam_cy,
+      cam_d0, cam_d1, cam_d2, cam_d3
+    );
+
+  }
+  else if (cam_model == "EquidistantCamera") {
+    int cam_width = 640;
+    int cam_height = 480;
+    double scale = 1.0;
+    double cam_fx = 0.0;
+    double cam_fy = 0.0;
+    double cam_cx = 0.0;
+    double cam_cy = 0.0;
+    double k1 = 0.0;
+    double k2 = 0.0;
+    double k3 = 0.0;
+    double k4 = 0.0;
+    
+    node->get_parameter(ns + ".cam_width", cam_width);
+    node->get_parameter(ns + ".cam_height", cam_height);
+    node->get_parameter(ns + ".scale", scale);
+    node->get_parameter(ns + ".cam_fx", cam_fx);
+    node->get_parameter(ns + ".cam_fy", cam_fy);
+    node->get_parameter(ns + ".cam_cx", cam_cx);
+    node->get_parameter(ns + ".cam_cy", cam_cy);
+    node->get_parameter(ns + ".k1", k1);
+    node->get_parameter(ns + ".k2", k2);
+    node->get_parameter(ns + ".k3", k3);
+    node->get_parameter(ns + ".k4", k4);
+
+    cam = new vk::EquidistantCamera(
+      cam_width, cam_height, scale,
+      cam_fx, cam_fy, cam_cx, cam_cy,
+      k1, k2, k3, k4
+    );
+  }
+  else if (cam_model == "Polynomial Camera") {
+    double cam_width = 640;
+    double cam_height = 480;
+    double scale = 1.0;
+    double cam_fx = 0.0;
+    double cam_fy = 0.0;
+    double cam_cx = 0.0;
+    double cam_cy = 0.0;
+    double cam_skew;
+    double k2 = 0.0;
+    double k3 = 0.0;
+    double k4 = 0.0;
+    double k5 = 0.0;
+    double k6 = 0.0;
+    double k7 = 0.0;
+    
+    node->get_parameter(ns + ".cam_width", cam_width);
+    node->get_parameter(ns + ".cam_height", cam_height);
+    // node->get_parameter(ns + ".scale", scale);
+    node->get_parameter(ns + ".cam_fx", cam_fx);
+    node->get_parameter(ns + ".cam_fy", cam_fy);
+    node->get_parameter(ns + ".cam_cx", cam_cx);
+    node->get_parameter(ns + ".cam_cy", cam_cy);
+    node->get_parameter(ns + ".cam_skew", cam_skew);
+    node->get_parameter(ns + ".k2", k2);
+    node->get_parameter(ns + ".k3", k3);
+    node->get_parameter(ns + ".k4", k4);
+    node->get_parameter(ns + ".k5", k5);
+    node->get_parameter(ns + ".k6", k6);
+    node->get_parameter(ns + ".k7", k7);
+    
+    cam = new vk::PolynomialCamera(
+      cam_width, cam_height,
+      cam_fx, cam_fy, cam_cx, cam_cy,
+      cam_skew, k2, k3, k4, k5, k6, k7
+    );
+  }
+  else if (cam_model == "ATAN") {
+    int cam_width = 640;
+    int cam_height = 480;
+    double cam_fx = 0.0;
+    double cam_fy = 0.0;
+    double cam_cx = 0.0;
+    double cam_cy = 0.0;
+    double cam_d0 = 0.0;
+
+    node->get_parameter(ns + ".cam_width", cam_width);
+    node->get_parameter(ns + ".cam_height", cam_height);
+    node->get_parameter(ns + ".cam_fx", cam_fx);
+    node->get_parameter(ns + ".cam_fy", cam_fy);
+    node->get_parameter(ns + ".cam_cx", cam_cx);
+    node->get_parameter(ns + ".cam_cy", cam_cy);
+    node->get_parameter(ns + ".cam_d0", cam_d0);
+
+    cam = new vk::ATANCamera(
+      cam_width, cam_height,
+      cam_fx, cam_fy, cam_cx, cam_cy,
+      cam_d0
+    );
+  }
+  else {
+    RCLCPP_ERROR(node->get_logger(), "Unknown camera model: %s", cam_model.c_str());
+    return false;
+  }
+  
+  return true;
+}
+
+/// Load multiple cameras from ROS 2 parameters
+bool loadFromRosNs(const rclcpp::Node *node, const std::string& ns, std::vector<vk::AbstractCamera*>& cam_list)
+{
+  int cam_num;
+  if (!node->get_parameter(ns + ".cam_num", cam_num)) {
+    RCLCPP_ERROR(node->get_logger(), "Camera number parameter not found: %s.cam_num", ns.c_str());
+    return false;
+  }
+
+  bool success = true;
+  for (int i = 0; i < cam_num; i++) {
+    std::string cam_ns = ns + ".cam_" + std::to_string(i);
+    vk::AbstractCamera *cam;
+    
+    if (!loadFromRosNs(node, cam_ns, cam)) {
+      RCLCPP_WARN(node->get_logger(), "Failed to load camera from namespace: %s", cam_ns.c_str());
+      success = false;
+    } else {
+      cam_list.push_back(cam);
+    }
+  }
+
+  return success;
+}
+
+} // namespace camera_loader
+} // namespace vk
+
+#endif // VIKIT_CAMERA_LOADER_H_
diff --git a/include/vikit/output_helper.h b/include/vikit/output_helper.h
new file mode 100644
index 00000000..516cde8f
--- /dev/null
+++ b/include/vikit/output_helper.h
@@ -0,0 +1,100 @@
+/*
+ * output_helper.h
+ *
+ *  Created on: Jan 20, 2013
+ *      Author: cforster
+ *  Updated for ROS 2 by: [Your Name]
+ */
+
+#ifndef VIKIT_OUTPUT_HELPER_H_
+#define VIKIT_OUTPUT_HELPER_H_
+
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Core>
+#include <sophus/se3.hpp>
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+namespace vk {
+namespace output_helper {
+
+using namespace std;
+using namespace Eigen;
+
+void
+publishTfTransform      (const Sophus::SE3d& T, const rclcpp::Time& stamp,
+                         const string& frame_id, const string& child_frame_id,
+                         tf2_ros::TransformBroadcaster& br);
+
+void
+publishPointMarker      (rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
+                         const Vector3d& pos,
+                         const string& ns,
+                         const rclcpp::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color,
+                         rclcpp::Duration lifetime = rclcpp::Duration(0, 0));
+
+void
+publishLineMarker       (rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
+                         const Vector3d& start,
+                         const Vector3d& end,
+                         const string& ns,
+                         const rclcpp::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color,
+                         rclcpp::Duration lifetime = rclcpp::Duration(0, 0));
+
+void
+publishArrowMarker      (rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
+                         const Vector3d& pos,
+                         const Vector3d& dir,
+                         double scale,
+                         const string& ns,
+                         const rclcpp::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color);
+
+void
+publishHexacopterMarker (rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
+                         const string& frame_id,
+                         const string& ns,
+                         const rclcpp::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color);
+
+void
+publishCameraMarker(rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
+                    const string& frame_id,
+                    const string& ns,
+                    const rclcpp::Time& timestamp,
+                    int id,
+                    double marker_scale,
+                    const Vector3d& color);
+
+void
+publishFrameMarker     (rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
+                        const Matrix3d& rot,
+                        const Vector3d& pos,
+                        const string& ns,
+                        const rclcpp::Time& timestamp,
+                        int id,
+                        int action,
+                        double marker_scale,
+                        rclcpp::Duration lifetime = rclcpp::Duration(0, 0));
+
+} // namespace output_helper
+} // namespace vk
+
+#endif /* VIKIT_OUTPUT_HELPER_H_ */
diff --git a/include/vikit/params_helper.h b/include/vikit/params_helper.h
new file mode 100644
index 00000000..375e7cf0
--- /dev/null
+++ b/include/vikit/params_helper.h
@@ -0,0 +1,48 @@
+#ifndef ROS_PARAMS_HELPER_H_
+#define ROS_PARAMS_HELPER_H_
+
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+
+namespace vk {
+
+inline
+bool hasParam(rclcpp::Node::SharedPtr node, const std::string& name)
+{
+  return node->has_parameter(name);
+}
+
+template<typename T>
+T getParam(rclcpp::Node::SharedPtr node, const std::string& name, const T& defaultValue)
+{
+  T v;
+  if (node->get_parameter(name, v)) 
+  {
+    RCLCPP_INFO(node->get_logger(), "Found parameter: %s, value: %s", name.c_str(), std::to_string(v).c_str());
+    return v;
+  }
+  else
+  {
+    RCLCPP_WARN(node->get_logger(), "Cannot find value for parameter: %s, assigning default: %s", name.c_str(), std::to_string(defaultValue).c_str());
+  }
+  return defaultValue;
+}
+
+template<typename T>
+T getParam(rclcpp::Node::SharedPtr node, const std::string& name)
+{
+  T v;
+  int i = 0;
+  while (!node->get_parameter(name, v)) 
+  {
+    RCLCPP_ERROR(node->get_logger(), "Cannot find value for parameter: %s, will try again.", name.c_str());
+    if ((i++) >= 5) return T();
+  }
+
+  RCLCPP_INFO(node->get_logger(), "Found parameter: %s, value: %s", name.c_str(), std::to_string(v).c_str());
+  return v;
+}
+
+} // namespace vk
+
+#endif // ROS_PARAMS_HELPER_H_
diff --git a/launch/mapping.launch.py b/launch/mapping.launch.py
new file mode 100644
index 00000000..4eba2db0
--- /dev/null
+++ b/launch/mapping.launch.py
@@ -0,0 +1,62 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+
+import os
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true', description='Launch RViz'
+    )
+
+    config = os.path.join(
+        get_package_share_directory('fastlivo2'),
+        'config',
+        'avia.yaml'
+    )
+
+    rviz_config = os.path.join(
+        get_package_share_directory('fastlivo2'),
+        'rviz',
+        'fast_livo2.rviz'
+    )
+
+    # Load parameters
+    slam_node = Node(
+        package='fastlivo2',
+        executable='fastlivo_mapping',
+        output='screen',
+        parameters=[config]
+    )
+
+    # RViz node
+    rviz_node = GroupAction([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz',
+            arguments=['-d', rviz_config],
+            output='screen'
+        )
+    ])
+
+    # Image transport republish node
+    image_republish = Node(
+        package='image_transport',
+        executable='republish',
+        name='republish',
+        arguments=['compressed', 'in:=/left_camera/image', 'raw', 'out:=/left_camera/image'],
+        output='screen',
+        respawn=True
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        slam_node,
+        rviz_node,
+        image_republish
+    ])
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/package.xml b/package.xml
old mode 100755
new mode 100644
index af3a353c..7048f685
--- a/package.xml
+++ b/package.xml
@@ -1,55 +1,31 @@
 <?xml version="1.0"?>
-<package>
-  <name>fast_livo</name>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fastlivo2</name>
   <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="daryllee@roboworks.co">daryl</maintainer>
+  <license>TODO: License declaration</license>
 
-  <description>
-    This is a modified version of LOAM which is original algorithm
-    is described in the following paper:
-    J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
-      Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
-  </description>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <maintainer email="dev@livoxtech.com">claydergc</maintainer>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
-  <license>BSD</license>
-
-  <author email="zhangji@cmu.edu">Ji Zhang</author>
-  
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>pcl_ros</build_depend>
-  <build_depend>livox_ros_driver</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>vikit_common</build_depend>
-  <build_depend>vikit_ros</build_depend>
-  <build_depend>cv_bridge</build_depend>
-
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>pcl_ros</run_depend>
-  <run_depend>livox_ros_driver</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <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>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_sensor_msgs</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_py</depend>
 
   <export>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>
diff --git a/rviz/fast_livo2.rviz b/rviz/fast_livo2.rviz
new file mode 100755
index 00000000..7857825f
--- /dev/null
+++ b/rviz/fast_livo2.rviz
@@ -0,0 +1,260 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /MarkerArray1/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 476
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 0; 0
+      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: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_registered
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 10000
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 150
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_registered
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /path
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /image_raw
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        aft_mapped:
+          Value: true
+        camera_init:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        camera_init:
+          aft_mapped:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        plane: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planes
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: camera_init
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        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_default_plugins/Orbit
+      Distance: 29.058467864990234
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.7602909803390503
+        Y: 4.272579669952393
+        Z: 10.99958610534668
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.18979667127132416
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.3753955364227295
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1011
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000355fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000267000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002aa000000e80000002800ffffff000000010000010f00000355fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000355000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027900fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000035500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 32
diff --git a/rviz_cfg/fast_livo2.rviz b/rviz_cfg/fast_livo2.rviz
deleted file mode 100755
index da2892e3..00000000
--- a/rviz_cfg/fast_livo2.rviz
+++ /dev/null
@@ -1,671 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 0
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Status1
-        - /Axes1
-        - /mapping1
-        - /mapping1/currPoints1
-        - /mapping1/surround1
-        - /mapping1/surround1/Autocompute Value Bounds1
-        - /mapping1/PointCloud21
-        - /Odometry1
-        - /Odometry1/Odometry1
-        - /Odometry1/Odometry1/Shape1
-        - /Path1
-        - /currPoints1/Autocompute Value Bounds1
-        - /MarkerArray1/Namespaces1
-        - /currPoints2/Autocompute Value Bounds1
-        - /Odometry2/Shape1
-        - /MarkerArray3
-        - /MarkerArray4
-        - /MarkerArray5
-        - /Image1
-      Splitter Ratio: 0.34272301197052
-    Tree Height: 538
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Name: Time
-    SyncMode: 0
-    SyncSource: surround
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 1
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: false
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 160
-      Reference Frame: <Fixed Frame>
-      Value: false
-    - Alpha: 1
-      Class: rviz/Axes
-      Enabled: true
-      Length: 0.699999988079071
-      Name: Axes
-      Radius: 0.10000000149011612
-      Reference Frame: <Fixed Frame>
-      Show Trail: false
-      Value: true
-    - Class: rviz/Group
-      Displays:
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 20
-            Min Value: -3
-            Value: false
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 239; 41; 41
-          Color Transformer: FlatColor
-          Decay Time: 0
-          Enabled: true
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Min Color: 0; 0; 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
-          Use Fixed Frame: true
-          Use rainbow: false
-          Value: true
-        - Alpha: 1
-          Autocompute Intensity Bounds: false
-          Autocompute Value Bounds:
-            Max Value: 15
-            Min Value: -5
-            Value: false
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 238; 238; 236
-          Color Transformer: RGB8
-          Decay Time: 10000
-          Enabled: true
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 159
-          Min Color: 0; 0; 0
-          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
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: true
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 239; 41; 41
-          Color Transformer: ""
-          Decay Time: 0
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Min Color: 0; 0; 0
-          Name: PointCloud2
-          Position Transformer: XYZ
-          Queue Size: 10
-          Selectable: true
-          Size (Pixels): 5
-          Size (m): 0.019999999552965164
-          Style: Squares
-          Topic: /cloud_effected
-          Unreliable: false
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-      Enabled: true
-      Name: mapping
-    - Class: rviz/Group
-      Displays:
-        - Angle Tolerance: 0.009999999776482582
-          Class: rviz/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: true
-          Keep: 1
-          Name: Odometry
-          Position Tolerance: 0.0010000000474974513
-          Queue Size: 10
-          Shape:
-            Alpha: 1
-            Axes Length: 0.5
-            Axes Radius: 0.15000000596046448
-            Color: 255; 85; 0
-            Head Length: 0
-            Head Radius: 0
-            Shaft Length: 0.800000011920929
-            Shaft Radius: 0.5
-            Value: Axes
-          Topic: /aft_mapped_to_init
-          Unreliable: false
-          Value: true
-      Enabled: true
-      Name: Odometry
-    - Alpha: 0
-      Buffer Length: 2
-      Class: rviz/Path
-      Color: 25; 255; 255
-      Enabled: true
-      Head Diameter: 0
-      Head Length: 0
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.03999999910593033
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        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
-      Value: true
-    - Alpha: 0.10000000149011612
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 20
-        Min Value: -3
-        Value: false
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 239; 41; 41
-      Color Transformer: Intensity
-      Decay Time: 1000
-      Enabled: false
-      Invert Rainbow: true
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 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
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /planner_normal
-      Name: Marker
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /voxels
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 245; 121; 0
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 1
-      Selectable: true
-      Size (Pixels): 15
-      Size (m): 0.009999999776482582
-      Style: Points
-      Topic: /cloud_ray_sub_map
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /visualization_marker
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 92; 53; 102
-      Color Transformer: FlatColor
-      Decay Time: 99999
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 10
-      Size (m): 0.009999999776482582
-      Style: Points
-      Topic: /cloud_visual_map
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 115; 210; 22
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 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
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 20
-        Min Value: -3
-        Value: false
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 237; 212; 0
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 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
-      Use Fixed Frame: true
-      Use rainbow: false
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 239; 41; 41
-      Color Transformer: FlatColor
-      Decay Time: 99999
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 239; 41; 41
-      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
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 92; 53; 102
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 20
-      Size (m): 0.009999999776482582
-      Style: Points
-      Topic: /cloud_ray_sub_map_fov
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Angle Tolerance: 0
-      Class: rviz/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: false
-      Keep: 1
-      Name: Odometry
-      Position Tolerance: 0
-      Queue Size: 10
-      Shape:
-        Alpha: 1
-        Axes Length: 0.699999988079071
-        Axes Radius: 0.20000000298023224
-        Color: 255; 25; 0
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Axes
-      Topic: /aft_mapped_to_init
-      Unreliable: false
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /waypoint_planner/visualize
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /fsm_node/visualization/exp_traj
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /fsm_node/visualization/exp_sfcs
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Class: rviz/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
-      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
-      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
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/ThirdPersonFollower
-      Distance: 25.669620513916016
-      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
-        Z: -3.4865479392465204e-05
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: -0.10520334541797638
-      Target Frame: drone
-      Yaw: 3.161787748336792
-    Saved:
-      - Class: rviz/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
-          Z: 4.459701061248779
-        Focal Shape Fixed Size: false
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
-        Name: far1
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 0.19539840519428253
-        Target Frame: <Fixed Frame>
-        Yaw: 0.17540442943572998
-      - Class: rviz/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
-          Z: 14.125411987304688
-        Focal Shape Fixed Size: false
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
-        Name: far2
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 0.035398442298173904
-        Target Frame: <Fixed Frame>
-        Yaw: 5.793589115142822
-      - Class: rviz/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
-          Z: -36.31112289428711
-        Focal Shape Fixed Size: false
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
-        Name: near1
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 0.5653983950614929
-        Target Frame: <Fixed Frame>
-        Yaw: 0.9104044437408447
-      - Class: rviz/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
-          Z: -12.290168762207031
-        Focal Shape Fixed Size: false
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
-        Name: near2
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 0.315398633480072
-        Target Frame: <Fixed Frame>
-        Yaw: 5.788588047027588
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 1376
-  Hide Left Dock: false
-  Hide Right Dock: false
-  Image:
-    collapsed: false
-  QMainWindow State: 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
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 2488
-  X: 72
-  Y: 27
diff --git a/src/IMU_Processing.cpp b/src/IMU_Processing.cpp
index b9306242..85f871a7 100755
--- a/src/IMU_Processing.cpp
+++ b/src/IMU_Processing.cpp
@@ -10,13 +10,32 @@ 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 "IMU_Processing.h"
+#include <cassert>
+#include <omp.h>
+
+#include "fastlivo2/IMU_Processing.h"
 
 const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); }
 
-ImuProcess::ImuProcess() : Eye3d(M3D::Identity()),
-                           Zero3d(0, 0, 0), b_first_frame(true), imu_need_init(true)
+double ros2TimeToDouble(const builtin_interfaces::msg::Time &time_msg) {
+  return static_cast<double>(time_msg.sec) + static_cast<double>(time_msg.nanosec) * 1e-9;
+}
+  
+builtin_interfaces::msg::Time doubleToRos2Time(double time_double) {
+  builtin_interfaces::msg::Time time_msg;
+  time_msg.sec = static_cast<int32_t>(time_double);
+  time_msg.nanosec = static_cast<uint32_t>((time_double - time_msg.sec) * 1e9);
+  return time_msg;
+}
+
+ImuProcess::ImuProcess()
+  : Eye3d(M3D::Identity()),
+    Zero3d(0, 0, 0),
+    b_first_frame(true),
+    imu_need_init(true),
+    logger(rclcpp::get_logger("IMU_Processing"))
 {
+  // logger = ;;
   init_iter_num = 1;
   cov_acc = V3D(0.1, 0.1, 0.1);
   cov_gyr = V3D(0.1, 0.1, 0.1);
@@ -29,7 +48,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 +56,14 @@ ImuProcess::~ImuProcess() {}
 
 void ImuProcess::Reset()
 {
-  ROS_WARN("Reset ImuProcess");
+  RCLCPP_WARN(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 +126,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(logger, "IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
   V3D cur_acc, cur_gyr;
 
   if (b_first_frame)
@@ -225,8 +244,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 = ros2TimeToDouble(v_imu.front()->header.stamp);
+  const double &imu_end_time = ros2TimeToDouble(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());
@@ -287,7 +306,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 = 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;
@@ -311,7 +330,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 (ros2TimeToDouble(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);
@@ -326,30 +345,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) << ros2TimeToDouble(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 = 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 (ros2TimeToDouble(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 = ros2TimeToDouble(tail->header.stamp) - last_prop_end_time;
+        offs_t = ros2TimeToDouble(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 = ros2TimeToDouble(tail->header.stamp) - ros2TimeToDouble(head->header.stamp);
+        offs_t = ros2TimeToDouble(tail->header.stamp) - prop_beg_time;
       }
       else
       {
         // printf("22 \n");
-        dt = prop_end_time - head->header.stamp.toSec();
+        dt = prop_end_time - ros2TimeToDouble(head->header.stamp);
         offs_t = prop_end_time - prop_beg_time;
       }
 
@@ -406,8 +425,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<<"tail->header.stamp:
+      // "<<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));
     }
@@ -526,7 +545,7 @@ void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, Poin
 {
   double t1, t2, t3;
   t1 = omp_get_wtime();
-  ROS_ASSERT(lidar_meas.lidar != nullptr);
+  assert(lidar_meas.lidar != nullptr);
   if (!imu_en)
   {
     Forward_without_imu(lidar_meas, stat, *cur_pcl_un_);
@@ -552,11 +571,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(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(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 a26fd2ca..ed7b457f 100755
--- a/src/LIVMapper.cpp
+++ b/src/LIVMapper.cpp
@@ -10,10 +10,31 @@ 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 "LIVMapper.h"
+#include <rclcpp/rclcpp.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <omp.h>
 
-LIVMapper::LIVMapper(ros::NodeHandle &nh)
-    : extT(0, 0, 0),
+#include <vikit/camera_loader.h>
+
+#include "fastlivo2/LIVMapper.h"
+#include "fastlivo2/voxel_map.h"
+
+double ros2TimeToDouble2(const builtin_interfaces::msg::Time &time_msg) {
+  return static_cast<double>(time_msg.sec) + static_cast<double>(time_msg.nanosec) * 1e-9;
+}
+  
+builtin_interfaces::msg::Time doubleToRos2Time2(double time_double) {
+  builtin_interfaces::msg::Time time_msg;
+  time_msg.sec = static_cast<int32_t>(time_double);
+  time_msg.nanosec = static_cast<uint32_t>((time_double - time_msg.sec) * 1e9);
+  return time_msg;
+}
+
+
+LIVMapper::LIVMapper(rclcpp::NodeOptions options)
+    : Node("fastlivo2_node", options),
+      extT(0, 0, 0),
       extR(M3D::Identity())
 {
   extrinT.assign(3, 0.0);
@@ -24,9 +45,9 @@ LIVMapper::LIVMapper(ros::NodeHandle &nh)
   p_pre.reset(new Preprocess());
   p_imu.reset(new ImuProcess());
 
-  readParameters(nh);
+  readParameters();
   VoxelMapConfig voxel_config;
-  loadVoxelConfig(nh, voxel_config);
+  loadVoxelConfig(this, voxel_config);
 
   visual_sub_map.reset(new PointCloudXYZI());
   feats_undistort.reset(new PointCloudXYZI());
@@ -38,73 +59,77 @@ LIVMapper::LIVMapper(ros::NodeHandle &nh)
   voxelmap_manager.reset(new VoxelMapManager(voxel_config, voxel_map));
   vio_manager.reset(new VIOManager());
   root_dir = ROOT_DIR;
+
   initializeFiles();
   initializeComponents();
-  path.header.stamp = ros::Time::now();
+  path.header.stamp = this->get_clock()->now();
   path.header.frame_id = "camera_init";
 }
 
 LIVMapper::~LIVMapper() {}
 
-void LIVMapper::readParameters(ros::NodeHandle &nh)
+void LIVMapper::readParameters()
 {
-  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);
+  this->get_parameter("common.lid_topic", lid_topic);
+  this->get_parameter("common.imu_topic", imu_topic);
+  this->get_parameter("common.img_en", img_en);
+  this->get_parameter("common.lidar_en", lidar_en);
+  this->get_parameter("common.img_topic", img_topic);
+  this->get_parameter("common.ros_driver_bug_fix", ros_driver_fix_en);
+
+  this->get_parameter("vio.normal_en", normal_en);
+  this->get_parameter("vio.inverse_composition_en", inverse_composition_en);
+  this->get_parameter("vio.max_iterations", max_iterations);
+  this->get_parameter("vio.img_point_cov", IMG_POINT_COV);
+  this->get_parameter("vio.raycast_en", raycast_en);
+  this->get_parameter("vio.exposure_estimate_en", exposure_estimate_en);
+  this->get_parameter("vio.inv_expo_cov", inv_expo_cov);
+  this->get_parameter("vio.grid_size", grid_size);
+  this->get_parameter("vio.grid_n_height", grid_n_height);
+  this->get_parameter("vio.patch_pyrimid_level", patch_pyrimid_level);
+  this->get_parameter("vio.patch_size", patch_size);
+  this->get_parameter("vio.outlier_threshold", outlier_threshold);
+
+  this->get_parameter("time_offset.exposure_time_init", exposure_time_init);
+  this->get_parameter("time_offset.img_time_offset", img_time_offset);
+  this->get_parameter("time_offset.imu_time_offset", imu_time_offset);
+
+  this->get_parameter("uav.imu_rate_odom", imu_prop_enable);
+  this->get_parameter("uav.gravity_align_en", gravity_align_en);
+  
+  this->get_parameter("evo.seq_name", seq_name);
+  this->get_parameter("evo.pose_output_en", pose_output_en);
+
+  this->get_parameter("imu.gyr_cov", gyr_cov);
+  this->get_parameter("imu.acc_cov", acc_cov);
+  this->get_parameter("imu.imu_int_frame", imu_int_frame);
+  this->get_parameter("imu.imu_en", imu_en);
+  this->get_parameter("imu.gravity_est_en", gravity_est_en);
+  this->get_parameter("imu.ba_bg_est_en", ba_bg_est_en);
+
+  this->get_parameter("preprocess.blind", p_pre->blind);
+  this->get_parameter("preprocess.filter_size_surf", filter_size_surf_min);
+  this->get_parameter("preprocess.lidar_type", p_pre->lidar_type);
+  this->get_parameter("preprocess.scan_line", p_pre->N_SCANS);
+  this->get_parameter("preprocess.point_filter_num", p_pre->point_filter_num);
+  this->get_parameter("preprocess.feature_extract_enabled", p_pre->feature_enabled);
+
+  this->get_parameter("pcd_save.interval", pcd_save_interval);
+  this->get_parameter("pcd_save.pcd_save_en", pcd_save_en);
+  this->get_parameter("pcd_save.colmap_output_en", colmap_output_en);
+  this->get_parameter("pcd_save.filter_size_pcd", filter_size_pcd);
+
+  this->get_parameter("extrin_calib.extrinsic_T", extrinT);
+  this->get_parameter("extrin_calib.extrinsic_R", extrinR);
+  this->get_parameter("extrin_calib.Pcl", cameraextrinT);
+  this->get_parameter("extrin_calib.Rcl", cameraextrinR);
+  this->get_parameter("debug.plot_time", plot_time);
+  this->get_parameter("debug.frame_cnt", frame_cnt);
+
+  this->get_parameter("publish.blind_rgb_points", blind_rgb_points);
+  this->get_parameter("publish.pub_scan_num", pub_scan_num);
+  this->get_parameter("publish.pub_effect_point_en", pub_effect_point_en);
+  this->get_parameter("publish.dense_map_en", dense_map_en);
 
   p_pre->blind_sqr = p_pre->blind * p_pre->blind;
 }
@@ -117,8 +142,7 @@ void LIVMapper::initializeComponents()
 
   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, "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;
@@ -181,31 +205,33 @@ 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(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);
+  sub_pcl = this->create_subscription<livox_interfaces::msg::CustomMsg>(lid_topic, 200000, std::bind(&LIVMapper::livox_pcl_cbk, this, std::placeholders::_1));
+  // sub_pcl = this->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, 200000, std::bind(&LIVMapper::standard_pcl_cbk, this, std::placeholders::_1));
+  sub_imu = this->create_subscription<sensor_msgs::msg::Imu>(imu_topic, 200000, std::bind(&LIVMapper::imu_cbk, this, std::placeholders::_1));
+  sub_img = this->create_subscription<sensor_msgs::msg::Image>(img_topic, 200000, std::bind(&LIVMapper::img_cbk, this, std::placeholders::_1));
+
+  pubLaserCloudFullRes = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered", 100);
+  pubNormal = this->create_publisher<visualization_msgs::msg::MarkerArray>("visualization_marker", 100);
+  pubSubVisualMap = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_visual_sub_map_before", 100);
+  pubLaserCloudEffect = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_effected", 100);
+  pubLaserCloudMap = this->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 100);
+  pubOdomAftMapped = this->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 10);
+  pubPath = this->create_publisher<nav_msgs::msg::Path>("/path", 10);
+  plane_pub = this->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1);
+  voxel_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("/voxels", 1);
+  pubLaserCloudDyn = this->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj", 100);
+  pubLaserCloudDynRmed = this->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj_removed", 100);
+  pubLaserCloudDynDbg = this->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj_dbg_hist", 100);
+  mavros_pose_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>("/mavros/vision_pose/pose", 10);
+  pubImuPropOdom = this->create_publisher<nav_msgs::msg::Odometry>("/LIVO2/imu_propagate", 10000);
   
-  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);
-  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);
+  imu_prop_timer = this->create_wall_timer(std::chrono::milliseconds(4), std::bind(&LIVMapper::imu_prop_callback, this));
+  
+  pubImage = std::make_shared<image_transport::Publisher>(it.advertise("/image_raw", 1));
+  tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+  voxelmap_manager->voxel_map_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/planes", 10000);
 }
 
 void LIVMapper::handleFirstFrame() 
@@ -315,8 +341,8 @@ void LIVMapper::handleVIO()
   //   visual_sub_map->push_back(temp_map);
   // }
 
-  publish_frame_world(pubLaserCloudFullRes, vio_manager);
-  publish_img_rgb(pubImage, vio_manager);
+  publish_frame_world();
+  publish_img_rgb();
 
   euler_cur = RotMtoEuler(_state.rot_end);
   fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " "
@@ -331,8 +357,7 @@ void LIVMapper::handleLIO()
            << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " "
            << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << endl;
            
-  if (feats_undistort->empty() || (feats_undistort == nullptr)) 
-  {
+  if (feats_undistort->empty() || (feats_undistort == nullptr)) {
     std::cout << "[ LIO ]: No point!!!" << std::endl;
     return;
   }
@@ -343,7 +368,7 @@ void LIVMapper::handleLIO()
   downSizeFilterSurf.filter(*feats_down_body);
   
   double t_down = omp_get_wtime();
-
+  
   feats_down_size = feats_down_body->points.size();
   voxelmap_manager->feats_down_body_ = feats_down_body;
   transformLidar(_state.rot_end, _state.pos_end, feats_down_body, feats_down_world);
@@ -353,9 +378,9 @@ void LIVMapper::handleLIO()
   if (!lidar_map_inited) 
   {
     lidar_map_inited = true;
-    voxelmap_manager->BuildVoxelMap();
+    voxelmap_manager->BuildVoxelMap(); //segfault here
   }
-
+  
   double t1 = omp_get_wtime();
 
   voxelmap_manager->StateEstimation(state_propagat);
@@ -381,12 +406,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->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->get_logger(), "open fail\n");
     }
     Eigen::Matrix4d outT;
     Eigen::Quaterniond q(_state.rot_end);
@@ -396,8 +421,10 @@ void LIVMapper::handleLIO()
   }
   
   euler_cur = RotMtoEuler(_state.rot_end);
-  geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2));
-  publish_odometry(pubOdomAftMapped);
+  tf2::Quaternion q;
+  q.setRPY(euler_cur(0), euler_cur(1), euler_cur(2));
+  geoQuat = tf2::toMsg(q);
+  publish_odometry();
 
   double t3 = omp_get_wtime();
 
@@ -433,11 +460,11 @@ void LIVMapper::handleLIO()
   }
   *pcl_w_wait_pub = *laserCloudWorld;
 
-  if (!img_en) publish_frame_world(pubLaserCloudFullRes, vio_manager);
-  if (pub_effect_point_en) publish_effect_world(pubLaserCloudEffect, voxelmap_manager->ptpl_list_);
+  if (!img_en) publish_frame_world();
+  if (pub_effect_point_en) publish_effect_world(voxelmap_manager->ptpl_list_);
   if (voxelmap_manager->config_setting_.is_pub_plane_map_) voxelmap_manager->pubVoxelMap();
-  publish_path(pubPath);
-  publish_mavros(mavros_pose_publisher);
+  publish_path();
+  publish_mavros();
 
   frame_num++;
   aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t4 - t0) / frame_num;
@@ -516,26 +543,19 @@ void LIVMapper::savePCD()
   }
 }
 
+// make timer 
 void LIVMapper::run() 
 {
-  ros::Rate rate(5000);
-  while (ros::ok()) 
-  {
-    ros::spinOnce();
-    if (!sync_packages(LidarMeasures)) 
-    {
-      rate.sleep();
-      continue;
-    }   
-    handleFirstFrame();
-
-    processImu();
-
-    // if (!p_imu->imu_time_init) continue;
-
-    stateEstimationAndMapping();
+  if (!sync_packages(LidarMeasures)) {
+    auto rate = rclcpp::Rate(5000);
+    rate.sleep();
+    return;
   }
-  savePCD();
+  
+  handleFirstFrame();
+  processImu();
+  // if (!p_imu->imu_time_init) continue;
+  stateEstimationAndMapping();
 }
 
 void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr)
@@ -558,7 +578,7 @@ 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();
@@ -570,16 +590,16 @@ 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() && ros2TimeToDouble2(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 = ros2TimeToDouble2(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;
+        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);
         V3D omg_imu(prop_imu_buffer[i].angular_velocity.x, prop_imu_buffer[i].angular_velocity.y, prop_imu_buffer[i].angular_velocity.z);
         prop_imu_once(imu_propagate, dt, acc_imu, omg_imu);
@@ -591,7 +611,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 = ros2TimeToDouble2(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;
@@ -614,7 +634,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();
 }
@@ -674,62 +694,65 @@ 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 (ros2TimeToDouble2(msg->header.stamp) < last_timestamp_lidar)
   {
-    ROS_ERROR("lidar loop back, clear buffer");
+    RCLCPP_ERROR(this->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->get_logger(), "get point cloud at time: %.6f", 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(ros2TimeToDouble2(msg->header.stamp));
+  last_timestamp_lidar = ros2TimeToDouble2(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_interfaces::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)
+  auto msg = std::make_shared<livox_interfaces::msg::CustomMsg>(*msg_in);
+  // if ((abs(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);
+  //   RCLCPP_WARN(this->get_logger(), "lidar jumps %.3f\n", msg->header.stamp - last_timestamp_lidar);
   //   sync_jump_flag = true;
   //   msg->header.stamp = ros::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 - ros2TimeToDouble2(msg->header.stamp)) > 1.0 && !imu_buffer.empty())
   {
-    double timediff_imu_wrt_lidar = last_timestamp_imu - msg->header.stamp.toSec();
+    double timediff_imu_wrt_lidar = last_timestamp_imu - ros2TimeToDouble2(msg->header.stamp);
     printf("\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 = ros2TimeToDouble2(msg->header.stamp);
+  RCLCPP_INFO(this->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->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->get_logger(), "get point cloud at time: %.6f", 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->get_logger(), "Received an empty point cloud");
     mtx_buffer.unlock();
     return;
   }
 
+  // save as pcd
+  pcl::io::savePCDFileASCII ("test_pcd.pcd", *ptr);
+
   lid_raw_data_buffer.push_back(ptr);
   lid_header_time_buffer.push_back(cur_head_time);
   last_timestamp_lidar = cur_head_time;
@@ -738,23 +761,22 @@ 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();
+  sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in));
+  double timestamp = ros2TimeToDouble2(msg->header.stamp) - imu_time_offset;
+  msg->header.stamp = doubleToRos2Time2(timestamp);
 
   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->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 = doubleToRos2Time2(timestamp); // Change to nanosecs
 
   mtx_buffer.lock();
 
@@ -762,14 +784,14 @@ 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->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);
+  //   RCLCPP_WARN(this->get_logger(), "imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu);
   //   mtx_buffer.unlock();
   //   sig_buffer.notify_all();
   //   return;
@@ -791,7 +813,7 @@ 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;
@@ -799,13 +821,13 @@ cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
 }
 
 // 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(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->get_logger(), "img jumps %.3f\n", msg->header.stamp - last_timestamp_img);
   //   sync_jump_flag = true;
   //   msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1);
   // }
@@ -816,15 +838,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 =  msg->header.stamp.sec;
+  double msg_header_time = ros2TimeToDouble2(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->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->get_logger(), "image loop back. \n");
     return;
   }
 
@@ -834,7 +856,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->get_logger(), "Image need Jumps: %.6f", img_time_correct);
     mtx_buffer.unlock();
     sig_buffer.notify_all();
     return;
@@ -844,7 +866,7 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in)
   img_buffer.push_back(img_cur);
   img_time_buffer.push_back(img_time_correct);
 
-  // ROS_INFO("Correct Image time: %.6f", img_time_correct);
+  // RCLCPP_INFO(this->get_logger(), "Correct Image time: %.6f", img_time_correct);
 
   last_timestamp_img = img_time_correct;
   // cv::imshow("img", img);
@@ -852,6 +874,7 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in)
   // cout<<"last_timestamp_img:::"<<last_timestamp_img<<endl;
   mtx_buffer.unlock();
   sig_buffer.notify_all();
+  // std::cout << "Image time: " << img_time_correct << std::endl;
 }
 
 bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
@@ -881,7 +904,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
     { // waiting imu message needs to be
       // larger than _lidar_frame_end_time,
       // make sure complete propagate.
-      // ROS_ERROR("out sync");
+      // RCLCPP_ERROR(this->get_logger(), "out sync");
       return false;
     }
 
@@ -892,7 +915,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 (ros2TimeToDouble2(imu_buffer.front()->header.stamp) > meas.lidar_frame_end_time) break;
       m.imu.push_back(imu_buffer.front());
       imu_buffer.pop_front();
     }
@@ -903,7 +926,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
 
     meas.lio_vio_flg = LIO; // process lidar topic, so timestamp should be lidar scan end.
     meas.measures.push_back(m);
-    // ROS_INFO("ONlY HAS LiDAR and IMU, NO IMAGE!");
+    // RCLCPP_INFO(this->get_logger(), "ONlY HAS LiDAR and IMU, NO IMAGE!");
     lidar_pushed = false; // sync one whole lidar scan.
     return true;
 
@@ -933,19 +956,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 = ros2TimeToDouble2(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->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->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;
@@ -960,13 +983,15 @@ 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 (ros2TimeToDouble2(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 (ros2TimeToDouble2(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());
+        // imu_buffer.front()->header.stamp.);
       }
       mtx_buffer.unlock();
       sig_buffer.notify_all();
@@ -1021,7 +1046,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 = ros2TimeToDouble2(imu_buffer.front()->header.stamp);
 
       struct MeasureGroup m;
       m.vio_time = img_capture_time;
@@ -1030,12 +1055,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 = imu_buffer.front()->header.stamp.sec;
       //   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());
+      //   imu_buffer.front()->header.stamp.);
       // }
       img_buffer.pop_front();
       img_time_buffer.pop_front();
@@ -1063,21 +1088,21 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas)
     return false;
   }
   }
-  ROS_ERROR("out sync");
+  RCLCPP_ERROR(this->get_logger(), "out sync");
 }
 
-void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager)
+void LIVMapper::publish_img_rgb()
 {
   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->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());
+  pubImage->publish(out_msg.toImageMsg());
 }
 
-void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager)
+void LIVMapper::publish_frame_world()
 {
   if (pcl_w_wait_pub->empty()) return;
   PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB());
@@ -1127,7 +1152,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;
@@ -1137,9 +1162,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->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
@@ -1178,7 +1203,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()
 {
   PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map);
   int size = laserCloudFullRes->points.size(); if (size == 0) return;
@@ -1186,15 +1211,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->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 std::vector<PointToPlane> &ptpl_list)
 {
   int effect_feat_num = ptpl_list.size();
   PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1));
@@ -1204,11 +1229,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->get_clock()->now();
   laserCloudFullRes3.header.frame_id = "camera_init";
-  pubLaserCloudEffect.publish(laserCloudFullRes3);
+  pubLaserCloudEffect->publish(laserCloudFullRes3);
 }
 
 template <typename T> void LIVMapper::set_posestamp(T &out)
@@ -1222,39 +1247,51 @@ 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()
 {
   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->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)));
-  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);
+  // 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);
+
+  auto msg = geometry_msgs::msg::TransformStamped();
+  msg.header.stamp = odomAftMapped.header.stamp;
+  msg.header.frame_id = "camera_init";
+  msg.child_frame_id = "aft_mapped";
+  msg.transform.translation.x = _state.pos_end(0);
+  msg.transform.translation.y = _state.pos_end(1);
+  msg.transform.translation.z = _state.pos_end(2);
+  msg.transform.rotation.x = geoQuat.x;
+  msg.transform.rotation.y = geoQuat.y;
+  msg.transform.rotation.z = geoQuat.z;
+  msg.transform.rotation.w = geoQuat.w;
+
+  tf_broadcaster->sendTransform(msg);
+  pubOdomAftMapped->publish(odomAftMapped);
 }
 
-void LIVMapper::publish_mavros(const ros::Publisher &mavros_pose_publisher)
+void LIVMapper::publish_mavros()
 {
-  msg_body_pose.header.stamp = ros::Time::now();
+  msg_body_pose.header.stamp = this->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()
 {
   set_posestamp(msg_body_pose.pose);
-  msg_body_pose.header.stamp = ros::Time::now();
+  msg_body_pose.header.stamp = this->get_clock()->now();
   msg_body_pose.header.frame_id = "camera_init";
   path.poses.push_back(msg_body_pose);
-  pubPath.publish(path);
+  pubPath->publish(path);
 }
\ No newline at end of file
diff --git a/src/frame.cpp b/src/frame.cpp
index 2a0b1268..f6162b5a 100644
--- a/src/frame.cpp
+++ b/src/frame.cpp
@@ -10,15 +10,16 @@ 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 "feature.h"
-#include "frame.h"
-#include "visual_point.h"
 #include <stdexcept>
+
 #include <vikit/math_utils.h>
 #include <vikit/performance_monitor.h>
 #include <vikit/vision.h>
 
+#include "fastlivo2/feature.h"
+#include "fastlivo2/frame.h"
+#include "fastlivo2/visual_point.h"
+
 int Frame::frame_counter_ = 0;
 
 Frame::Frame(vk::AbstractCamera *cam, const cv::Mat &img)
diff --git a/src/main.cpp b/src/main.cpp
index 135b3363..962cc9b8 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,12 +1,32 @@
-#include "LIVMapper.h"
+#include <rclcpp/rclcpp.hpp>
+#include <image_transport/image_transport.hpp>
+
+#include "fastlivo2/LIVMapper.h"
 
 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();
-  return 0;
+    rclcpp::init(argc, argv);
+
+    // Allow undeclared params
+    rclcpp::NodeOptions options;
+    options.allow_undeclared_parameters(true);
+    options.automatically_declare_parameters_from_overrides(true);
+
+    auto node = std::make_shared<LIVMapper>(options);
+
+    auto it = std::make_shared<image_transport::ImageTransport>(node);
+    node->initializeSubscribersAndPublishers(*it);
+    
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node);
+
+    while (rclcpp::ok())
+    {
+        executor.spin_once();
+        node->run();
+    }
+    node->savePCD();
+
+    rclcpp::shutdown();
+    return 0;
 }
\ No newline at end of file
diff --git a/src/preprocess.cpp b/src/preprocess.cpp
index 322776b0..22f455fe 100755
--- a/src/preprocess.cpp
+++ b/src/preprocess.cpp
@@ -10,12 +10,19 @@ 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 "preprocess.h"
+#include <livox_interfaces/msg/custom_msg.hpp>
+
+#include "fastlivo2/preprocess.h"
 
 #define RETURN0 0x00
 #define RETURN0AND1 0x10
 
-Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
+Preprocess::Preprocess()
+  : point_filter_num(1),
+    feature_enabled(0),
+    lidar_type(AVIA),
+    blind(0.01),
+    rclcpp::Node("Preprocess")
 {
   inf_bound = 10;
   N_SCANS = 6;
@@ -51,13 +58,13 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
   point_filter_num = pfilt_num;
 }
 
-void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
+void Preprocess::process(const livox_interfaces::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out)
 {
   avia_handler(msg);
   *pcl_out = pl_surf;
 }
 
-void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
+void Preprocess::process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out)
 {
   switch (lidar_type)
   {
@@ -87,7 +94,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_interfaces::msg::CustomMsg::ConstSharedPtr &msg)
 {
   pl_surf.clear();
   pl_corn.clear();
@@ -195,7 +202,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 +213,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 = msg->header.stamp.sec;
   // 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 +242,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 +304,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   }
   else
   {
-    double time_stamp = msg->header.stamp.toSec();
+    double time_stamp = msg->header.stamp.sec;
     // 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 +342,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();
@@ -502,7 +509,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 +560,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 +940,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;
diff --git a/src/vio.cpp b/src/vio.cpp
index 2ff8fd0f..90dabfeb 100755
--- a/src/vio.cpp
+++ b/src/vio.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 "vio.h"
+#include "fastlivo2/vio.h"
 
 VIOManager::VIOManager()
 {
@@ -42,28 +42,28 @@ void VIOManager::initializeVIO()
 {
   visual_submap = new SubSparseMap;
 
-  fx = cam->fx();
+  fx = cam->fx(); //possible error accessing fx
   fy = cam->fy();
   cx = cam->cx();
   cy = cam->cy();
   image_resize_factor = cam->scale();
-
+  
   printf("intrinsic: %.6lf, %.6lf, %.6lf, %.6lf\n", fx, fy, cx, cy);
-
+  
   width = cam->width();
   height = cam->height();
-
+  
   printf("width: %d, height: %d, scale: %f\n", width, height, image_resize_factor);
   Rci = Rcl * Rli;
   Pci = Rcl * Pli + Pcl;
-
+  
   V3D Pic;
   M3D tmp;
   Jdphi_dR = Rci;
   Pic = -Rci.transpose() * Pci;
   tmp << SKEW_SYM_MATRX(Pic);
   Jdp_dR = -Rci * tmp;
-
+  
   if (grid_size > 10)
   {
     grid_n_width = ceil(static_cast<double>(width / grid_size));
@@ -76,7 +76,7 @@ void VIOManager::initializeVIO()
     grid_n_width = ceil(static_cast<double>(width / grid_size));
   }
   length = grid_n_width * grid_n_height;
-
+  
   if(raycast_en)
   {
     // cv::Mat img_test = cv::Mat::zeros(height, width, CV_8UC1);
diff --git a/src/visual_point.cpp b/src/visual_point.cpp
index 1b746509..a97174d1 100644
--- a/src/visual_point.cpp
+++ b/src/visual_point.cpp
@@ -10,11 +10,12 @@ 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 "visual_point.h"
-#include "feature.h"
 #include <stdexcept>
 #include <vikit/math_utils.h>
 
+#include "fastlivo2/visual_point.h"
+#include "fastlivo2/feature.h"
+
 VisualPoint::VisualPoint(const Vector3d &pos)
     : pos_(pos), previous_normal_(Vector3d::Zero()), normal_(Vector3d::Zero()),
       is_converged_(false), is_normal_initialized_(false), has_ref_patch_(false)
diff --git a/src/voxel_map.cpp b/src/voxel_map.cpp
index 01923cca..67fff272 100644
--- a/src/voxel_map.cpp
+++ b/src/voxel_map.cpp
@@ -10,7 +10,11 @@ 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 "voxel_map.h"
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "fastlivo2/voxel_map.h"
 
 void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov)
 {
@@ -33,23 +37,24 @@ 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* node, VoxelMapConfig &voxel_config)
 {
-  nh.param<bool>("publish/pub_plane_en", voxel_config.is_pub_plane_map_, false);
+  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.max_points_num", voxel_config.max_points_num_);
+  node->get_parameter("lio.max_iterations", voxel_config.max_iterations_);
   
-  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);
+  voxel_config.layer_init_num_ = node->get_parameter("lio.layer_init_num").as_integer_array();
+
+  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)
@@ -490,7 +495,10 @@ void VoxelMapManager::StateEstimation(StatesGroup &state_propagat)
           (I_STATE.block<DIM_STATE, DIM_STATE>(0, 0) - G.block<DIM_STATE, DIM_STATE>(0, 0)) * state_.cov.block<DIM_STATE, DIM_STATE>(0, 0);
       // total_distance += (_state.pos_end - position_last).norm();
       position_last_ = state_.pos_end;
-      geoQuat_ = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2));
+
+      tf2::Quaternion quaternion;
+      quaternion.setRPY(euler_cur(0), euler_cur(1), euler_cur(2));
+      geoQuat_ = tf2::toMsg(quaternion);;
 
       // VD(DIM_STATE) K_sum  = K.rowwise().sum();
       // VD(DIM_STATE) P_diag = _state.cov.diagonal();
@@ -535,10 +543,9 @@ 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<int64_t> layer_init_num = config_setting_.layer_init_num_;
 
   std::vector<pointWithVar> input_points;
-
   for (size_t i = 0; i < feats_down_world_->size(); i++)
   {
     pointWithVar pv;
@@ -553,7 +560,6 @@ void VoxelMapManager::BuildVoxelMap()
     pv.var = var;
     input_points.push_back(pv);
   }
-
   uint plsize = input_points.size();
   for (uint i = 0; i < plsize; i++)
   {
@@ -573,6 +579,10 @@ void VoxelMapManager::BuildVoxelMap()
     }
     else
     {
+      std::cout << "max_layer: " << max_layer << std::endl;
+      std::cout << "layer_init_num[0]: " << layer_init_num[0] << std::endl;
+      std::cout << "max_points_num: " << max_points_num << std::endl;
+      std::cout << "planer_threshold: " << planer_threshold << std::endl;
       VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold);
       voxel_map_[position] = octo_tree;
       voxel_map_[position]->quater_length_ = voxel_size / 4;
@@ -612,7 +622,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<int64_t> layer_init_num = config_setting_.layer_init_num_;
   uint plsize = input_points.size();
   for (uint i = 0; i < plsize; i++)
   {
@@ -640,8 +650,9 @@ void VoxelMapManager::UpdateVoxelMap(const std::vector<pointWithVar> &input_poin
   }
 }
 
-void VoxelMapManager::BuildResidualListOMP(std::vector<pointWithVar> &pv_list, std::vector<PointToPlane> &ptpl_list)
-{
+void VoxelMapManager::BuildResidualListOMP(
+  std::vector<pointWithVar> &pv_list, std::vector<PointToPlane> &ptpl_list
+) {
   int max_layer = config_setting_.max_layer_;
   double voxel_size = config_setting_.max_voxel_size_;
   double sigma_num = config_setting_.sigma_num_;
@@ -722,8 +733,10 @@ void VoxelMapManager::build_single_residual(pointWithVar &pv, const VoxelOctoTre
   {
     VoxelPlane &plane = *current_octo->plane_ptr_;
     Eigen::Vector3d p_world_to_center = p_w - plane.center_;
-    float dis_to_plane = fabs(plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_);
-    float dis_to_center = (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) +
+    float dis_to_plane = fabs(plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) +
+                              plane.normal_(2) * p_w(2) + plane.d_);
+    float dis_to_center = (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + 
+                          (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) +
                           (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2));
     float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane);
 
@@ -789,11 +802,13 @@ 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;
+  std::cout << "voxel_map_.size(): " << voxel_map_.size() << std::endl;
+
   for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++)
   {
     GetUpdatePlane(iter->second, config_setting_.max_layer_, pub_plane_list);
@@ -813,7 +828,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 +849,24 @@ 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,
-                                     const float alpha, const Eigen::Vector3d rgb)
+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::Clock().now();
   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 +876,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(0,0);
   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);