Add support for ROS2, test with Ubuntu 20.04 + ROS2 Galactic

.gitignore

@@ -0,0 +1,4 @@
+/.idea/
+/cmake-build-debug/
+*.pcd
+/Log/pos_log.csv

CMakeLists.txt

@@ -1,83 +1,99 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(point_lio)
 
-SET(CMAKE_BUILD_TYPE "Debug")
-
-ADD_COMPILE_OPTIONS(-std=c++14 )
-ADD_COMPILE_OPTIONS(-std=c++14 )
-set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 
-
-add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
-
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
-set(CMAKE_CXX_STANDARD 14)
+# Use C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
+
+# Compiler settings for performance and threading
+add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
+add_compile_options(-O3 -pthread -fexceptions)
 
-message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
+# Processor count and definitions
+message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}")
 if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
   include(ProcessorCount)
   ProcessorCount(N)
-  message("Processer number:  ${N}")
+  message(STATUS "Processor number:  ${N}")
   if(N GREATER 5)
     add_definitions(-DMP_EN)
     add_definitions(-DMP_PROC_NUM=4)
-    message("core for MP:  3")
+    message(STATUS "core for MP:  3")
   elseif(N GREATER 3)
     math(EXPR PROC_NUM "${N} - 2")
     add_definitions(-DMP_EN)
     add_definitions(-DMP_PROC_NUM="${PROC_NUM}")
-    message("core for MP:  ${PROC_NUM}")
+    message(STATUS "core for MP:  ${PROC_NUM}")
   else()
     add_definitions(-DMP_PROC_NUM=1)
   endif()
 else()
   add_definitions(-DMP_PROC_NUM=1)
 endif()
 
-find_package(OpenMP QUIET)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")
+# Find packages
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
 
-find_package(PythonLibs REQUIRED)
-find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
+find_package(Eigen3 REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  roscpp
-  rospy
-  std_msgs
-  pcl_ros
-  tf
-  livox_ros_driver
-  message_generation
-  eigen_conversions
-)
 
-find_package(Eigen3 REQUIRED)
-find_package(PCL 1.8 REQUIRED)
+# OpenMP
+find_package(OpenMP QUIET)
+if(OPENMP_FOUND)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+endif()
 
-message(Eigen: ${EIGEN3_INCLUDE_DIR})
+find_package(PythonLibs REQUIRED) # Consider using Pybind11 for ROS2
+find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
 
 include_directories(
-	${catkin_INCLUDE_DIRS} 
-  ${EIGEN3_INCLUDE_DIR}
-  ${PCL_INCLUDE_DIRS}
-  ${PYTHON_INCLUDE_DIRS}
-  include)
-
-catkin_package(
-  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
-  DEPENDS EIGEN3 PCL
-  INCLUDE_DIRS
+        include
+        ${EIGEN3_INCLUDE_DIR}
+        ${PYTHON_INCLUDE_DIRS}
 )
 
+# Declare a ROS2 executable
 add_executable(pointlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp)
-target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+ament_target_dependencies(pointlio_mapping
+        rclcpp
+        rclpy
+        geometry_msgs
+        nav_msgs
+        sensor_msgs
+        pcl_ros
+        pcl_conversions
+        tf2_ros
+        livox_ros_driver2
+)
+target_link_libraries(pointlio_mapping ${PYTHON_LIBRARIES})
 target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
 
+# Install the executable
+install(TARGETS
+        pointlio_mapping
+        DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+        DIRECTORY config launch rviz_cfg
+        DESTINATION share/${PROJECT_NAME}
+)
 
+# Export dependencies
+ament_export_dependencies(rclcpp rclpy geometry_msgs nav_msgs sensor_msgs pcl_ros pcl_conversions tf2_ros livox_ros_driver2 Eigen3)
 
+ament_package()

README.md

@@ -1,6 +1,8 @@
 # Point-LIO (If you have problems with the formal commits, please try the newest commit first, several modifications have be made in newest commit for wider adaptivity)
 ## Point-LIO: Robust High-Bandwidth Lidar-Inertial Odometry (Pay attention to modifying the parameters for IMU in .yaml file, according to the IMU you use.)
 
+ROS2 version of Point-LIO
+
 ## 1. Introduction
 
 <div align="center">
@@ -63,7 +65,7 @@ Our accompany video is available on **YouTube**.
 # **3. Prerequisites**
 
 ## **3.1 Ubuntu and [ROS](https://www.ros.org/)**
-We tested our code on Ubuntu20.04 with noetic. Ubuntu18.04 and lower versions have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems. Additional ROS package is required:
+We tested our code on Ubuntu 20.04 with Galactic. Ubuntu18.04 and lower versions have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems. Additional ROS package is required:
 ```
 sudo apt-get install ros-xxx-pcl-conversions
 ```
@@ -74,26 +76,26 @@ Following the official [Eigen installation](eigen.tuxfamily.org/index.php?title=
 sudo apt-get install libeigen3-dev
 ```
 
-## **3.3 livox_ros_driver**
-Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).
+## **3.3 livox_ros_driver2**
+Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
 
 *Remarks:*
-- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver** must be installed and **sourced** before run any Point-LIO luanch file.
-- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
+- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver2** must be installed and **sourced** before run any Point-LIO launch file.
+- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/install/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
 
 ## 4. Build
-Clone the repository and catkin_make:
+Clone the repository and colcon build:
 
 ```
-    cd ~/$A_ROS_DIR$/src
+    cd ~/$Point_LIO_ROS_DIR$/src
     git clone https://github.com/hku-mars/Point-LIO.git
     cd Point-LIO
     git submodule update --init
     cd ../..
-    catkin_make
-    source devel/setup.bash
+    colcon build --symlink-install
+    source install/setup.bash
 ```
-- Remember to source the livox_ros_driver before build (follow 3.3 **livox_ros_driver**)
+- Remember to source the livox_ros_driver before build (follow 3.3 **livox_ros_driver2**)
 - If you want to use a custom build of PCL, add the following line to ~/.bashrc
 ```export PCL_ROOT={CUSTOM_PCL_PATH}```
 
@@ -103,12 +105,12 @@ Clone the repository and catkin_make:
 Connect to your PC to Livox Avia LiDAR by following  [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
 ```
     cd ~/$Point_LIO_ROS_DIR$
-    source devel/setup.bash
-    roslaunch point_lio mapping_avia.launch
-    roslaunch livox_ros_driver livox_lidar_msg.launch
+    source install/setup.bash
+    ros2 launch point_lio mapping_avia.launch.py
+    ros2 launch livox_ros_driver msg_HAP_launch.py
 ```
-- For livox serials, Point-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch ``` can not produce it right now.
-- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage.
+- For livox serials, Point-LIO only support the data collected by the ``` msg_HAP_launch.py ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch.py ``` can not produce it right now.
+- If you want to change the frame rate, please modify the **publish_freq** parameter in the [msg_HAP_launch.py](https://github.com/Livox-SDK/livox_ros_driver2/blob/master/launch_ROS2/msg_HAP_launch.py) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage.
 
 ### 5.2 For Livox serials with external IMU
 
@@ -143,8 +145,8 @@ Edit ``` config/velodyne.yaml ``` to set the below parameters:
 Step B: Run below
 ```
     cd ~/$Point_LIO_ROS_DIR$
-    source devel/setup.bash
-    roslaunch point_lio mapping_velody16.launch
+    source install/setup.bash
+    ros2 launch point_lio mapping_velody16.launch.py
 ```
 
 Step C: Run LiDAR's ros driver or play rosbag.

config/avia.yaml

@@ -1,57 +1,59 @@
-common:
-    lid_topic:  "/livox/lidar" 
-    imu_topic:  "/livox/imu" 
-    con_frame: false # true: if you need to combine several LiDAR frames into one
-    con_frame_num: 1 # the number of frames combined
-    cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
-    cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
-    time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
-                               # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/livox/lidar"
+            imu_topic: "/livox/imu"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
 
-preprocess:
-    lidar_type: 1 
-    scan_line: 6
-    timestamp_unit: 1           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-    blind: 1.0 
+        preprocess:
+            lidar_type: 1
+            scan_line: 6
+            timestamp_unit: 1           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 1.0
 
-mapping:
-    imu_en: true
-    start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
-    extrinsic_est_en: false # for aggressive motion, set this variable false
-    imu_time_inte: 0.005 # = 1 / frequency of IMU
-    satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
-    satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
-    acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
-    lidar_meas_cov: 0.001 # 0.001; 0.01
-    acc_cov_output: 500
-    gyr_cov_output: 1000 
-    b_acc_cov: 0.0001 
-    b_gyr_cov: 0.0001 
-    imu_meas_acc_cov: 0.1 #0.1 # 0.1
-    imu_meas_omg_cov: 0.1 #0.01 # 0.1
-    gyr_cov_input: 0.01 # for IMU as input model
-    acc_cov_input: 0.1 # for IMU as input model
-    plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
-    match_s: 81
-    fov_degree: 90 
-    det_range: 450.0
-    gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
-    gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned
-    gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
-    extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
-    extrinsic_R: [ 1, 0, 0,
-                   0, 1, 0,
-                   0, 0, 1 ]
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
+            satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.001 # 0.001; 0.01
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.1 #0.1 # 0.1
+            imu_meas_omg_cov: 0.1 #0.01 # 0.1
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 90.0
+            det_range: 450.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ]
 
-odometry: 
-    publish_odometry_without_downsample: false
+        odometry:
+            publish_odometry_without_downsample: false
 
-publish:
-    path_en: true                 # false: close the path output
-    scan_publish_en: true         # false: close all the point cloud output
-    scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
 
-pcd_save:
-    pcd_save_en: false
-    interval: -1                 # how many LiDAR frames saved in each pcd file; 
-                                 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
+        pcd_save:
+            pcd_save_en: false
+            interval: -1                 # how many LiDAR frames saved in each pcd file;
+            # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.