[Feat] migrate code from ros1 to ros2

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.10)
 project(fast_livo)
 
 set(CMAKE_BUILD_TYPE "Release")
@@ -75,66 +75,101 @@ else()
   message(STATUS "mimalloc not found, proceeding without it")
 endif()
 
-# Find catkin and required dependencies
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  roscpp
-  rospy
-  std_msgs
-  pcl_ros
-  tf
-  message_generation
-  eigen_conversions
-  vikit_common
-  vikit_ros
-  cv_bridge
-  image_transport
-)
-
+# Find ament and required dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
+find_package(vikit_common REQUIRED)
+find_package(vikit_ros REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
-
-set(Sophus_LIBRARIES libSophus.so)
-
-# 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
-)
+find_package(fmt REQUIRED)
 
 # Include directories for dependencies
 include_directories(
-  ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
   ${Sophus_INCLUDE_DIRS}
+  ${vikit_common_INCLUDE_DIRS}
+  ${vikit_ros_INCLUDE_DIRS}
   include
 )
 
+set(dependencies
+  rclcpp
+  rclpy
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  visualization_msgs
+  cv_bridge
+  vikit_common
+  vikit_ros
+  image_transport
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  livox_ros_driver2
+)
+
+set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX fmt::fmt)
+
+# link_directories(${COMMON_DEPENDENCIES}
+#   ${vikit_common_LIBRARIES}/libvikit_common.so
+#   ${vikit_ros_LIBRARIES}/libvikit_ros.so
+# )
+
 # Add libraries
 add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp)
 add_library(lio src/voxel_map.cpp)
 add_library(pre src/preprocess.cpp)
 add_library(imu_proc src/IMU_Processing.cpp)
 add_library(laser_mapping src/LIVMapper.cpp)
+add_library(utils src/utils.cpp)
+
+ament_target_dependencies(vio ${dependencies} )
+ament_target_dependencies(lio ${dependencies})
+ament_target_dependencies(pre ${dependencies})
+ament_target_dependencies(imu_proc ${dependencies})
+ament_target_dependencies(laser_mapping ${dependencies})
+
+# linking libraries or executables to public dependencies
+target_link_libraries(laser_mapping 
+    ${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so 
+    ${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so
+    ${COMMON_DEPENDENCIES}
+)
+target_link_libraries(vio ${COMMON_DEPENDENCIES})
+target_link_libraries(lio utils ${COMMON_DEPENDENCIES})
+target_link_libraries(pre ${COMMON_DEPENDENCIES})
+target_link_libraries(imu_proc ${COMMON_DEPENDENCIES})
 
 # Add the main executable
 add_executable(fastlivo_mapping src/main.cpp)
 
+ament_target_dependencies(fastlivo_mapping ${dependencies})
+
 # Link libraries to the executable
 target_link_libraries(fastlivo_mapping
   laser_mapping
   vio
   lio
   pre
   imu_proc
-  ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${Sophus_LIBRARIES}
@@ -144,4 +179,34 @@ target_link_libraries(fastlivo_mapping
 # Link mimalloc if found
 if(mimalloc_FOUND)
   target_link_libraries(fastlivo_mapping mimalloc)
-endif()
+endif()
+
+# Install the executable
+install(TARGETS
+  fastlivo_mapping
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY config launch rviz_cfg
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Export dependencies
+ament_export_dependencies(
+  rclcpp
+  rclpy
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  livox_ros_driver2
+  Eigen3
+  PCL
+  OpenCV 
+  Sophus
+)
+
+ament_package()

README.md

@@ -1,7 +1,9 @@
-# FAST-LIVO2
+# FAST-LIVO2 ROS2 HUMBLE
 
 ## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry
 
+Thanks to hku mars lab chunran zheng for the open source excellent work
+
 ### 📢 News
 
 - 🔓 **2025-01-23**: Code released!  
@@ -46,11 +48,14 @@ Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoin
 ### 1.5 Our LiDAR-camera calibration method
 The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file. 
 
+### MARS-LVIG dataset
+[**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)：A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion
+
 ## 2. Prerequisited
 
 ### 2.1 Ubuntu and ROS
 
-Ubuntu 18.04~20.04.  [ROS Installation](http://wiki.ros.org/ROS/Installation).
+Ubuntu 22.04.  [ROS Installation](http://wiki.ros.org/ROS/Installation).
 
 ### 2.2 PCL && Eigen && OpenCV
 
@@ -62,6 +67,12 @@ OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/).
 
 ### 2.3 Sophus
 
+#### Binary installation
+```bash
+sudo apt install ros-$ROS_DISTRO-sophus
+```
+
+#### Building from source
 Sophus Installation for the non-templated/double-only version.
 
 ```bash
@@ -73,37 +84,111 @@ make
 sudo make install
 ```
 
+if build fails due to `so2.cpp:32:26: error: lvalue required as left operand of assignment`, modify the code as follows:
+
+**so2.cpp**
+```diff
+namespace Sophus
+{
+
+SO2::SO2()
+{
+-  unit_complex_.real() = 1.;
+-  unit_complex_.imag() = 0.;
++  unit_complex_.real(1.);
++  unit_complex_.imag(0.);
+}
+```
+
 ### 2.4 Vikit
 
 Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder.
 
+For well-known reasons, ROS2 does not have a direct global parameter server and a simple method to obtain the corresponding parameters. For details, please refer to https://discourse.ros.org/t/ros2-global-parameter-server-status/10114/11. I use a special way to get camera parameters in Vikit. While the method I've provided so far is quite simple and not perfect, it meets my needs. More contributions to improve `rpg_vikit` are hoped.
+
 ```bash
 # Different from the one used in fast-livo1
-cd catkin_ws/src
-git clone https://github.com/xuankuzcr/rpg_vikit.git 
+cd fast_ws/src
+git clone https://github.com/Robotic-Developer-Road/rpg_vikit.git 
 ```
 
+Thanks to the following repositories for the code reference:
+
+- [uzh-rpg/rpg_vikit](https://github.com/uzh-rpg/rpg_vikit)
+- [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit)
+- [uavfly/vikit](https://github.com/uavfly/vikit)
+
+### 2.5 **livox_ros_driver2**
+
+Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
+
+why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient.
+
 ## 3. Build
 
-Clone the repository and catkin_make:
+Clone the repository and colcon build:
 
 ```
-cd ~/catkin_ws/src
-git clone https://github.com/hku-mars/FAST-LIVO2
+cd ~/fast_ws/src
+git clone https://github.com/Robotic-Developer-Road/FAST-LIVO2.git
 cd ../
-catkin_make
-source ~/catkin_ws/devel/setup.bash
+colcon build --symlink-install --continue-on-error
+source ~/fast_ws/install/setup.bash
 ```
 
 ## 4. Run our examples
 
 Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). 
 
+### convert rosbag
+
+convert ROS1 rosbag to ROS2 rosbag
+```bash
+pip install rosbags
+rosbags-convert --src Retail_Street.bag --dst Retail_Street
 ```
-roslaunch fast_livo mapping_avia.launch
-rosbag play YOUR_DOWNLOADED.bag
+- [gitlab rosbags](https://gitlab.com/ternaris/rosbags)
+- [pypi rosbags](https://pypi.org/project/rosbags/)
+
+### change the msg type on rosbag
+
+Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file. 
+1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2.
+2. change the msg type of msg type in **metadata.yaml** as follows:
+
+**metadata.yaml**
+```diff
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: {}
+  duration:
+    nanoseconds: 135470252209
+  files:
+  - duration:
+      nanoseconds: 135470252209
+    message_count: 30157
+    path: Retail_Street.db3
+    ..............
+    topic_metadata:
+      name: /livox/lidar
+      offered_qos_profiles: ''
+      serialization_format: cdr
+-     type: livox_ros_driver/msg/CustomMsg
++     type: livox_ros_driver2/msg/CustomMsg
+      type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d
+    ...............
+.....
 ```
 
+### Run the demo
+
+Do not forget to `source` your ROS2 workspace before running the following command.
+
+```bash
+ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True
+ros2 bag play -p Retail_Street  # space bar controls play/pause
+```
 
 ## 5. License