Skip to content

Commit 4365b37

Browse files
iserverobotics-bonaalexlin2
iserverobotics-bona
authored and
alexlin2
committed
add fast_lio_ros2
A few things before you can run it: To enable auto-relocalization, edit localizer.yaml: auto_relocalize: true default_pcd_path: "/path/to/your/map.pcd" initial_x: 1.0 initial_y: 2.0 initial_z: 0.0 initial_yaw: 1.57 # radians Expected Log Output [INFO] [localizer_node]: [AUTO-RELOC] Enabled with map: /path/to/map.pcd [INFO] [localizer_node]: [AUTO-RELOC] Initial pose: x=1.000, y=2.000, z=0.000, yaw=1.570 ... [INFO] [localizer_node]: [AUTO-RELOC] Triggering relocalization... [INFO] [localizer_node]: [AUTO-RELOC] Map: /path/to/map.pcd [INFO] [localizer_node]: [AUTO-RELOC] Map loaded successfully [INFO] [localizer_node]: [AUTO-RELOC] Initial guess set, waiting for localization... [INFO] [localizer_node]: [RELOCALIZE] SUCCESS - Localization converged Usage: FASTLIO2 only (SLAM): ros2 launch fastlio2 lio_launch.py FASTLIO2 + Localizer (with relocalization): ros2 launch localizer localizer_launch.py Both launch files include rviz2 automatically.
1 parent 0a90160 commit 4365b37

66 files changed

Lines changed: 7550 additions & 0 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎src/FASTLIO2_ROS2/README.md‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
# FASTLIO2 ROS2
2+
## 主要工作
3+
1. 重构[FASTLIO2](https://github.com/hku-mars/FAST_LIO) 适配ROS2
4+
2. 添加回环节点，基于位置先验+ICP进行回环检测，基于GTSAM进行位姿图优化
5+
3. 添加重定位节点，基于由粗到细两阶段ICP进行重定位
6+
4. 增加一致性地图优化，基于[BLAM](https://github.com/hku-mars/BALM) (小场景地图) 和[HBA](https://github.com/hku-mars/HBA) (大场景地图)
7+
8+
## 环境依赖
9+
1. Ubuntu 22.04
10+
2. ROS2 Humble
11+
12+
## 编译依赖
13+
```text
14+
pcl
15+
Eigen
16+
sophus
17+
gtsam
18+
livox_ros_driver2
19+
```
20+
21+
## 详细说明
22+
### 1.编译 LIVOX-SDK2
23+
```shell
24+
git clone https://github.com/Livox-SDK/Livox-SDK2.git
25+
cd ./Livox-SDK2/
26+
mkdir build
27+
cd build
28+
cmake .. && make -j
29+
sudo make install
30+
```
31+
32+
### 2.编译 livox_ros_driver2
33+
```shell
34+
mkdir -r ws_livox/src
35+
git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
36+
cd ws_livox/src/livox_ros_driver2
37+
source /opt/ros/humble/setup.sh
38+
./build.sh humble
39+
```
40+
41+
### 3.编译 Sophus
42+
```shell
43+
git clone https://github.com/strasdat/Sophus.git
44+
cd Sophus
45+
git checkout 1.22.10
46+
mkdir build && cd build
47+
cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON
48+
make
49+
sudo make install
50+
```
51+
52+
**新的Sophus依赖fmt，可以在CMakeLists.txt中添加add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)去除，否则会报错**
53+
54+
55+
## 实例数据集
56+
```text
57+
链接: https://pan.baidu.com/s/1rTTUlVwxi1ZNo7ZmcpEZ7A?pwd=t6yb 提取码: t6yb
58+
--来自百度网盘超级会员v7的分享
59+
```
60+
61+
## 部分脚本
62+
63+
### 1.激光惯性里程计
64+
```shell
65+
ros2 launch fastlio2 lio_launch.py
66+
ros2 bag play your_bag_file
67+
```
68+
69+
### 2.里程计加回环
70+
#### 启动回环节点
71+
```shell
72+
ros2 launch pgo pgo_launch.py
73+
ros2 bag play your_bag_file
74+
```
75+
#### 保存地图
76+
```shell
77+
ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}"
78+
```
79+
80+
### 3.里程计加重定位
81+
#### 启动重定位节点
82+
```shell
83+
ros2 launch localizer localizer_launch.py
84+
ros2 bag play your_bag_file // 可选
85+
```
86+
#### 设置重定位初始值
87+
```shell
88+
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}"
89+
```
90+
#### 检查重定位结果
91+
```shell
92+
ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}"
93+
```
94+
95+
### 4.一致性地图优化
96+
#### 启动一致性地图优化节点
97+
```shell
98+
ros2 launch hba hba_launch.py
99+
```
100+
#### 调用优化服务
101+
```shell
102+
ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}"
103+
```
104+
**如果需要调用优化服务，保存地图时需要设置save_patches为true**
105+
106+
## 特别感谢
107+
1. [FASTLIO2](https://github.com/hku-mars/FAST_LIO)
108+
2. [BLAM](https://github.com/hku-mars/BALM)
109+
3. [HBA](https://github.com/hku-mars/HBA)
110+
## 性能相关的问题
111+
该代码主要使用timerCB作为频率触发主函数，由于ROS2中的timer、subscriber以及service的回调实际上运行在同一个线程上，在电脑性能不是好的时候，会出现调用阻塞的情况，建议使用线程并发的方式将耗时的回调独立出来(如timerCB)来提升性能
112+

‎src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(fastlio2)
3+
4+
set(CMAKE_BUILD_TYPE "Release")
5+
set(CMAKE_CXX_STANDARD 17)
6+
set(CMAKE_CXX_STANDARD_REQUIRED True)
7+
8+
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
9+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions")
10+
11+
12+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
13+
add_compile_options(-O3)
14+
add_compile_options(-Wall -Wextra -Wpedantic)
15+
endif()
16+
17+
add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)
18+
19+
add_definitions(-DMP_EN)
20+
add_definitions(-DMP_PROC_NUM=2)
21+
22+
# find dependencies
23+
find_package(ament_cmake REQUIRED)
24+
find_package(rclcpp REQUIRED)
25+
find_package(std_msgs REQUIRED)
26+
find_package(tf2 REQUIRED)
27+
find_package(tf2_ros REQUIRED)
28+
find_package(nav_msgs REQUIRED)
29+
find_package(sensor_msgs REQUIRED)
30+
find_package(livox_ros_driver2 REQUIRED)
31+
find_package(pcl_conversions REQUIRED)
32+
find_package(geometry_msgs REQUIRED)
33+
34+
find_package(OpenMP QUIET)
35+
find_package(PCL REQUIRED)
36+
find_package(Eigen3 REQUIRED)
37+
find_package(Sophus REQUIRED)
38+
find_package(yaml-cpp REQUIRED)
39+
40+
41+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
42+
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
43+
44+
set(SRC_LIST src/map_builder/commons.cpp
45+
src/map_builder/ieskf.cpp
46+
src/map_builder/imu_processor.cpp
47+
src/map_builder/ikd_Tree.cpp
48+
src/map_builder/lidar_processor.cpp
49+
src/map_builder/map_builder.cpp
50+
src/utils.cpp)
51+
52+
add_executable(lio_node src/lio_node.cpp ${SRC_LIST})
53+
ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs)
54+
target_link_libraries(lio_node
55+
yaml-cpp
56+
${PCL_LIBRARIES}
57+
)
58+
59+
60+
install(TARGETS lio_node DESTINATION lib/${PROJECT_NAME})
61+
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
62+
install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
63+
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
64+
65+
66+
67+
68+
if(BUILD_TESTING)
69+
find_package(ament_lint_auto REQUIRED)
70+
# the following line skips the linter which checks for copyrights
71+
# comment the line when a copyright and license is added to all source files
72+
set(ament_cmake_copyright_FOUND TRUE)
73+
# the following line skips cpplint (only works in a git repo)
74+
# comment the line when this package is in a git repo and when
75+
# a copyright and license is added to all source files
76+
set(ament_cmake_cpplint_FOUND TRUE)
77+
ament_lint_auto_find_test_dependencies()
78+
endif()
79+
80+
ament_package()

‎src/FASTLIO2_ROS2/fastlio2/LICENSE‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
Permission is hereby granted, free of charge, to any person obtaining a copy
2+
of this software and associated documentation files (the "Software"), to deal
3+
in the Software without restriction, including without limitation the rights
4+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5+
copies of the Software, and to permit persons to whom the Software is
6+
furnished to do so, subject to the following conditions:
7+
8+
The above copyright notice and this permission notice shall be included in
9+
all copies or substantial portions of the Software.
10+
11+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
13+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
14+
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
15+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
16+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
17+
THE SOFTWARE.

‎src/FASTLIO2_ROS2/fastlio2/config/lio.yaml‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
imu_topic: /livox/imu
2+
lidar_topic: /livox/lidar
3+
body_frame: body
4+
world_frame: lidar
5+
print_time_cost: false
6+
print_odom: true
7+
odom_log_interval: 0.5 # seconds between odom logs (2Hz)
8+
9+
lidar_filter_num: 6
10+
lidar_min_range: 0.5
11+
lidar_max_range: 30.0
12+
scan_resolution: 0.15
13+
map_resolution: 0.3
14+
15+
cube_len: 300
16+
det_range: 60
17+
move_thresh: 1.5
18+
19+
na: 0.01
20+
ng: 0.01
21+
nba: 0.0001
22+
nbg: 0.0001
23+
24+
imu_init_num: 20
25+
near_search_num: 5
26+
ieskf_max_iter: 5
27+
28+
gravity_align: true
29+
esti_il: false
30+
31+
r_il: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
32+
t_il: [-0.011, -0.02329, 0.04412]
33+
34+
lidar_cov_inv: 1000.0

‎src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
import launch
2+
import launch_ros.actions
3+
from launch.substitutions import PathJoinSubstitution
4+
from launch_ros.substitutions import FindPackageShare
5+
6+
7+
def generate_launch_description():
8+
9+
rviz_cfg = PathJoinSubstitution(
10+
[FindPackageShare("fastlio2"), "rviz", "fastlio2.rviz"]
11+
)
12+
13+
config_path = PathJoinSubstitution(
14+
[FindPackageShare("fastlio2"), "config", "lio.yaml"]
15+
)
16+
17+
18+
return launch.LaunchDescription(
19+
[
20+
launch_ros.actions.Node(
21+
package="fastlio2",
22+
namespace="fastlio2",
23+
executable="lio_node",
24+
name="lio_node",
25+
output="screen",
26+
parameters=[{"config_path": config_path.perform(launch.LaunchContext())}]
27+
),
28+
launch_ros.actions.Node(
29+
package="rviz2",
30+
namespace="fastlio2",
31+
executable="rviz2",
32+
name="rviz2",
33+
output="screen",
34+
arguments=["-d", rviz_cfg.perform(launch.LaunchContext())],
35+
),
36+
]
37+
)

‎src/FASTLIO2_ROS2/fastlio2/package.xml‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>fastlio2</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="zhouzhou@todo.todo">zhouzhou</maintainer>
8+
<license>MIT</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>std_msgs</depend>
14+
<depend>tf2</depend>
15+
<depend>tf2_ros</depend>
16+
<depend>nav_msgs</depend>
17+
<depend>sensor_msgs</depend>
18+
<depend>pcl_conversions</depend>
19+
<depend>livox_ros_driver2</depend>
20+
<depend>geometry_msgs</depend>
21+
22+
23+
<test_depend>ament_lint_auto</test_depend>
24+
<test_depend>ament_lint_common</test_depend>
25+
26+
<export>
27+
<build_type>ament_cmake</build_type>
28+
</export>
29+
</package>

0 commit comments

Comments
 (0)