Made it work in ros2 humbe

CMakeLists.txt

@@ -1,53 +1,72 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(ig_lio)
 
-set(CMAKE_CXX_STANDARD 17)
+# Use C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 set(CMAKE_BUILD_TYPE Release)
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS} -Wall")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  rospy
-  std_msgs
-  roslib
-  pcl_ros
-  livox_ros_driver
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+add_compile_options(-Wall -Wextra -Wpedantic -O3 -g)
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(PCL 1.8 REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
 find_package(TBB REQUIRED)
 
+# Optionally, handle any custom find_package modules like 'glog.cmake'
 include(cmake/glog.cmake)
 
-# sophus
-include_directories(${PROJECT_SOURCE_DIR}/thirdparty/sophus)
-
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES gicp_lio
-#  CATKIN_DEPENDS roscpp rospy std_msgs
-#  DEPENDS system_lib
+# Include directories
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIR}
+  ${PCL_INCLUDE_DIRS}
+  ${TBB_INCLUDE_DIRS}
+  ${PROJECT_SOURCE_DIR}/thirdparty/sophus
 )
 
-include_directories(
-  include 
+set(dependencies
+  rclcpp
+  sensor_msgs
+  nav_msgs
+  geometry_msgs
+  tf2_ros
+  tf2
+  pcl_conversions
+  TBB
+  livox_ros_driver2
+  std_srvs
+  tf2_geometry_msgs
 )
 
-include_directories(SYSTEM
-  ${catkin_INCLUDE_DIRS}	
+# Declare a C++ executable
+add_executable(${PROJECT_NAME}_node
+  src/ig_lio_node.cpp
+  src/pointcloud_preprocess.cpp
+  src/lio.cpp
+  src/voxel_map.cpp
+  src/SymmetricEigenSolver.cpp
+  src/timer.cpp
+  src/faster_voxel_grid.cpp
 )
 
-add_executable(${PROJECT_NAME}_node 
-  src/ig_lio_node
+# Declare a C++ executable
+add_executable(${PROJECT_NAME}_keyframe_node
+  src/keyframe.cpp
   src/pointcloud_preprocess.cpp
   src/lio.cpp
   src/voxel_map.cpp
@@ -56,31 +75,59 @@ add_executable(${PROJECT_NAME}_node
   src/faster_voxel_grid.cpp
 )
 
+# Declare a C++ executable
+add_executable(${PROJECT_NAME}_map_node
+  src/map.cpp
+)
+
+
+
+
+# Specify libraries to link a library or executable target against
+ament_target_dependencies(${PROJECT_NAME}_node
+  ${dependencies}
+)
+
+# Specify libraries to link a library or executable target against
+ament_target_dependencies(${PROJECT_NAME}_map_node
+  ${dependencies}
+)
+# Specify libraries to link a library or executable target against
+ament_target_dependencies(${PROJECT_NAME}_keyframe_node
+  ${dependencies}
+)
+
 target_link_libraries(${PROJECT_NAME}_node
-  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${TBB_LIBRARIES}
   glog
   gflags
-  TBB::tbb
 )
-
-install(TARGETS ${PROJECT_NAME}_node
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+target_link_libraries(${PROJECT_NAME}_map_node
+  ${PCL_LIBRARIES}
+  ${TBB_LIBRARIES}
+  glog
+  gflags
+)
+target_link_libraries(${PROJECT_NAME}_keyframe_node
+  ${PCL_LIBRARIES}
+  ${TBB_LIBRARIES}
+  glog
+  gflags
 )
 
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
-  PATTERN ".svn" EXCLUDE
+# Install the executable
+install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_map_node ${PROJECT_NAME}_keyframe_node
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-install(DIRECTORY launch/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-  PATTERN ".svn" EXCLUDE)
+# Install various directory contents
+install(DIRECTORY include/
+  DESTINATION include
+)
 
-install(DIRECTORY config/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
-  PATTERN ".svn" EXCLUDE)
+install(DIRECTORY launch config rviz
+  DESTINATION share/${PROJECT_NAME}
+)
 
-install(DIRECTORY rviz/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz
-  PATTERN ".svn" EXCLUDE)
+ament_package()

config/bg_velodyne.yaml.BK

@@ -0,0 +1,38 @@
+/**:
+  ros__parameters:
+    lidar_topic: /points_raw
+    imu_topic: /imu/data
+    lidar_type: velodyne # livox velodyne ouster
+    min_radius: 1.0
+    max_radius: 150.0
+    point_filter_num: 6
+    time_scale: 1000.0 # nclt: 0.001 other: 1000.0
+    enable_ahrs_initalization: false
+    enable_acc_correct: false
+
+    scan_resolution: 0.5
+    voxel_map_resolution: 0.5
+    max_iterations: 10
+
+    acc_cov: 0.01
+    gyr_cov: 0.0001
+    ba_cov: 0.000001
+    bg_cov: 0.000001
+
+    gicp_constraints_gain: 100.0
+    point2plane_constraints_gain: 1000.0
+    enable_undistort: true
+    enable_outlier_rejection: true
+
+    gravity: 9.80665
+    init_ori_cov: 0.0001
+    inti_pos_cov: 0.0001
+    init_vel_cov: 100.0
+    init_ba_cov: 0.0001
+    init_bg_cov: 0.0001
+
+    # botanic graden
+    t_imu_lidar : [0.0584867781527745, 0.00840419966766332,0.168915521980526]
+    R_imu_lidar: [0.999678872580465, 0.0252865664429322, 0.00150422292234868,
+                  -0.0252723438960774, 0.999649431893338, -0.0078025434141585,
+                  -0.00170103929405540, 0.00776298237926191, 0.99996789371916]

config/diffbot_velodyne.yaml

@@ -0,0 +1,65 @@
+/**:
+  ros__parameters:
+    #for odom
+    odom/sendTF: false
+    #topic names 
+    odom/lidar_topic: /points_raw
+    odom/imu_topic: /imu/data #/imu_correct
+    odom/lidar_type: velodyne # livox velodyne ouster
+
+    #frames
+    odom/odom_frame: lio_odom
+    odom/robot_frame: base_link
+    odom/imu_frame: imu_link
+    odom/lidar_frame: velodyne
+    odom/min_radius: 0.1
+    odom/max_radius: 150.0
+    odom/point_filter_num: 6
+    odom/time_scale: 1.0 # nclt: 0.001 other: 1000.0
+    odom/enable_ahrs_initalization: false
+    odom/enable_acc_correct: false
+
+    odom/scan_resolution: 0.2
+    odom/voxel_map_resolution: 0.5
+    odom/max_iterations: 10
+
+    odom/acc_cov: 0.1
+    odom/gyr_cov: 0.1
+    odom/ba_cov: 0.000001
+    odom/bg_cov: 0.000001
+    odom/init_ori_cov: 0.0001
+    odom/init_pos_cov: 0.0001
+    odom/init_vel_cov: 100.0
+    odom/init_ba_cov: 0.0001
+    odom/init_bg_cov: 0.0001
+    odom/gravity: 9.80665
+
+    odom/gicp_constraints_gain: 100.0
+    odom/point2plane_constraints_gain: 1000.0
+    odom/enable_undistort: true
+    odom/enable_outlier_rejection: true
+
+
+
+
+    #for extrinsics
+    extrinsics/imu2lidar/t: [0.0, 0.0, 0.0]
+    extrinsics/imu2lidar/r: [1.0, 0.0, 0.0,
+                            0.0, 1.0, 0.0,
+                            0.0,  0.0, 1.0 ]
+
+    extrinsics/robot2lidar/t: [0.0, 0.0, 0.0]
+    extrinsics/robot2lidar/r: [1.0, 0.0, 0.0,
+                              0.0, 1.0, 0.0,
+                              0.0,  0.0, 1.0 ]
+
+    extrinsics/robot2imu/t: [0.0, 0.0, 0.0]
+    extrinsics/robot2imu/r: [1.0, 0.0, 0.0,
+                            0.0, 1.0, 0.0,
+                            0.0,  0.0, 1.0 ]
+
+    #for map
+    # map/save_map_path: /home/agilex/humble_ws #do not put / at the end of line
+    # map/map_name: jfe
+    map/map_frame: map
+

config/hesai.yaml

@@ -0,0 +1,62 @@
+/**:
+  ros__parameters:
+    #for odom
+    odom/debug: false
+    #topic names 
+    odom/lidar_topic: /points_raw
+    odom/imu_topic: /imu/data #/imu_correct
+    odom/lidar_type: hesai # livox velodyne ouster hesai
+
+    #frames
+    odom/odom_frame: lio_odom
+    odom/robot_frame: base_link
+    odom/imu_frame: imu_link
+    odom/lidar_frame: hesai_lidar
+    odom/min_radius: 0.1
+    odom/max_radius: 150.0
+    odom/point_filter_num: 6
+    odom/time_scale: 1.0 # nclt: 0.001 other: 1000.0
+    odom/enable_ahrs_initalization: false
+    odom/enable_acc_correct: false
+
+    odom/scan_resolution: 0.2
+    odom/voxel_map_resolution: 0.5
+    odom/max_iterations: 10
+
+    odom/acc_cov: 0.1
+    odom/gyr_cov: 0.1
+    odom/ba_cov: 0.000001
+    odom/bg_cov: 0.000001
+    odom/init_ori_cov: 0.0001
+    odom/init_pos_cov: 0.0001
+    odom/init_vel_cov: 100.0
+    odom/init_ba_cov: 0.0001
+    odom/init_bg_cov: 0.0001
+    odom/gravity: 9.80665
+
+    odom/gicp_constraints_gain: 100.0
+    odom/point2plane_constraints_gain: 1000.0
+    odom/enable_undistort: true
+    odom/enable_outlier_rejection: true
+
+
+    #for extrinsics
+    extrinsics/imu2lidar/t: [ 0.1634, 0., 0.116 ]
+    extrinsics/imu2lidar/r: [ 0., -1.,  0.,
+                              1.,  0.,  0.,
+                              0.,  0.,  1. ]
+
+    extrinsics/robot2lidar/t: [0.0, 0.0, 0.0]
+    extrinsics/robot2lidar/r: [1.0, 0.0, 0.0,
+                              0.0, 1.0, 0.0,
+                              0.0,  0.0, 1.0 ]
+
+    extrinsics/robot2imu/t: [0.0, 0.0, 0.0]
+    extrinsics/robot2imu/r: [1.0, 0.0, 0.0,
+                            0.0, 1.0, 0.0,
+                            0.0,  0.0, 1.0 ]
+
+    #for map
+    # map/save_map_path: /home/agilex/humble_ws #do not put / at the end of line
+    # map/map_name: jfe
+    map/map_frame: lio_odom