VectorRobotics · baishibona · Dec 16, 2025
diff --git a/src/FASTLIO2_ROS2/README.md b/src/FASTLIO2_ROS2/README.md
@@ -0,0 +1,112 @@
+# FASTLIO2 ROS2
+## 主要工作
+1. 重构[FASTLIO2](https://github.com/hku-mars/FAST_LIO) 适配ROS2
+2. 添加回环节点，基于位置先验+ICP进行回环检测，基于GTSAM进行位姿图优化
+3. 添加重定位节点，基于由粗到细两阶段ICP进行重定位
+4. 增加一致性地图优化，基于[BLAM](https://github.com/hku-mars/BALM) (小场景地图) 和[HBA](https://github.com/hku-mars/HBA) (大场景地图)
+
+## 环境依赖
+1. Ubuntu 22.04
+2. ROS2 Humble
+
+## 编译依赖
+```text
+pcl
+Eigen
+sophus
+gtsam
+livox_ros_driver2
+```
+
+## 详细说明
+### 1.编译 LIVOX-SDK2
+```shell
+git clone https://github.com/Livox-SDK/Livox-SDK2.git
+cd ./Livox-SDK2/
+mkdir build
+cd build
+cmake .. && make -j
+sudo make install
+```
+
+### 2.编译 livox_ros_driver2
+```shell
+mkdir -r ws_livox/src
+git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
+cd ws_livox/src/livox_ros_driver2
+source /opt/ros/humble/setup.sh
+./build.sh humble
+```
+
+### 3.编译 Sophus
+```shell
+git clone https://github.com/strasdat/Sophus.git
+cd Sophus
+git checkout 1.22.10
+mkdir build && cd build
+cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON
+make
+sudo make install
+```
+
+**新的Sophus依赖fmt，可以在CMakeLists.txt中添加add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)去除，否则会报错**
+
+
+## 实例数据集
+```text
+链接: https://pan.baidu.com/s/1rTTUlVwxi1ZNo7ZmcpEZ7A?pwd=t6yb 提取码: t6yb 
+--来自百度网盘超级会员v7的分享
+```
+
+## 部分脚本
+
+### 1.激光惯性里程计 
+```shell
+ros2 launch fastlio2 lio_launch.py
+ros2 bag play your_bag_file
+```
+
+### 2.里程计加回环
+#### 启动回环节点
+```shell
+ros2 launch pgo pgo_launch.py
+ros2 bag play your_bag_file
+```
+#### 保存地图
+```shell
+ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}"
+```
+
+### 3.里程计加重定位
+#### 启动重定位节点
+```shell
+ros2 launch localizer localizer_launch.py
+ros2 bag play your_bag_file // 可选
+```
+#### 设置重定位初始值
+```shell
+ros2 service call /localizer/relocalize interface/srv/Relocalize "{"pcd_path": "your_map.pcd", "x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0, "pitch": 0.0, "roll": 0.0}"
+```
+#### 检查重定位结果
+```shell
+ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}"
+```
+
+### 4.一致性地图优化
+#### 启动一致性地图优化节点
+```shell
+ros2 launch hba hba_launch.py
+```
+#### 调用优化服务
+```shell
+ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}"
+```
+**如果需要调用优化服务，保存地图时需要设置save_patches为true**
+
+## 特别感谢
+1. [FASTLIO2](https://github.com/hku-mars/FAST_LIO)
+2. [BLAM](https://github.com/hku-mars/BALM)
+3. [HBA](https://github.com/hku-mars/HBA)
+## 性能相关的问题
+该代码主要使用timerCB作为频率触发主函数，由于ROS2中的timer、subscriber以及service的回调实际上运行在同一个线程上，在电脑性能不是好的时候，会出现调用阻塞的情况，建议使用线程并发的方式将耗时的回调独立出来(如timerCB)来提升性能
+
diff --git a/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt b/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt
@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.8)
+project(fastlio2)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions")
+
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-O3)
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)
+
+add_definitions(-DMP_EN)
+add_definitions(-DMP_PROC_NUM=2)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+find_package(OpenMP QUIET)
+find_package(PCL REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Sophus REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")
+
+set(SRC_LIST src/map_builder/commons.cpp
+             src/map_builder/ieskf.cpp
+             src/map_builder/imu_processor.cpp
+             src/map_builder/ikd_Tree.cpp
+             src/map_builder/lidar_processor.cpp
+             src/map_builder/map_builder.cpp
+             src/utils.cpp)
+
+add_executable(lio_node src/lio_node.cpp ${SRC_LIST})
+ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs)
+target_link_libraries(lio_node
+  yaml-cpp
+  ${PCL_LIBRARIES}
+)
+
+
+install(TARGETS lio_node DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/FASTLIO2_ROS2/fastlio2/LICENSE b/src/FASTLIO2_ROS2/fastlio2/LICENSE
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
diff --git a/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml b/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml
@@ -0,0 +1,34 @@
+imu_topic: /livox/imu
+lidar_topic: /livox/lidar
+body_frame: body
+world_frame: lidar
+print_time_cost: false
+print_odom: true
+odom_log_interval: 0.5  # seconds between odom logs (2Hz)
+
+lidar_filter_num: 6
+lidar_min_range: 0.5
+lidar_max_range: 30.0
+scan_resolution: 0.15
+map_resolution: 0.3
+
+cube_len: 300
+det_range: 60
+move_thresh: 1.5
+
+na: 0.01
+ng: 0.01
+nba: 0.0001
+nbg: 0.0001
+
+imu_init_num: 20
+near_search_num: 5
+ieskf_max_iter: 5
+
+gravity_align: true
+esti_il: false
+
+r_il: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+t_il: [-0.011, -0.02329, 0.04412]
+
+lidar_cov_inv: 1000.0
diff --git a/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py b/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py
@@ -0,0 +1,37 @@
+import launch
+import launch_ros.actions
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    rviz_cfg = PathJoinSubstitution(
+        [FindPackageShare("fastlio2"), "rviz", "fastlio2.rviz"]
+    )
+
+    config_path = PathJoinSubstitution(
+        [FindPackageShare("fastlio2"), "config", "lio.yaml"]
+    )
+
+
+    return launch.LaunchDescription(
+        [
+            launch_ros.actions.Node(
+                package="fastlio2",
+                namespace="fastlio2",
+                executable="lio_node",
+                name="lio_node",
+                output="screen",
+                parameters=[{"config_path": config_path.perform(launch.LaunchContext())}]
+            ),
+            launch_ros.actions.Node(
+                package="rviz2",
+                namespace="fastlio2",
+                executable="rviz2",
+                name="rviz2",
+                output="screen",
+                arguments=["-d", rviz_cfg.perform(launch.LaunchContext())],
+            ),
+        ]
+    )
diff --git a/src/FASTLIO2_ROS2/fastlio2/package.xml b/src/FASTLIO2_ROS2/fastlio2/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fastlio2</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="zhouzhou@todo.todo">zhouzhou</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>livox_ros_driver2</depend>
+  <depend>geometry_msgs</depend>
+
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>