# NEXTE_Sentry_Nav
**Repository Path**: huxiubin/NEXTE_Sentry_Nav
## Basic Information
- **Project Name**: NEXTE_Sentry_Nav
- **Description**: fast_lio_nav
- **Primary Language**: Unknown
- **License**: MIT
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-01-06
- **Last Updated**: 2025-01-06
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
# SLAM AND NAVIGATION IN RM2024
## Introduction
本文原本只是自己在拿到mid360后，开始进行开发过程的一些问题和学习的记录。毕竟实物和仿真还是有很多不同，且由于碰到的问题也比较多，READEME也越来越详细，所以就干脆整合起来，做成了一篇使用mid360的搭建入门的导航系统全流程分享。里面用到的都是主流的开源的框架（部分文件做了修改和mid360适配），fast_lio, move_base等等，或许能帮助到第一次开发机器人实物导航的朋友。
本文的代码地址：https://github.com/66Lau/NEXTE_Sentry_Nav
如果需要仿真环境：https://github.com/66Lau/sentry_sim
环境：
- ros-noetic
- ubuntu 20.04
你可以跟着下文步骤，逐一对clone开源仓库，再进行修改配置，但是建议直接直接克隆本仓库至你的工作空间的src下（因为做了很多修改，如果再clone我引用的原来的仓库，可能有些地方我没记在README里面导致你运行失败）然后再根据本文的顺序逐一进行配置和尝试
## Hardware info
- [MID360 offical web官网](https://www.livoxtech.com/cn/mid-360)
- [Quick-start-doc|MID360快速开始手册](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Mid360/Livox_Mid-360_Quick_Start_Guide_multi.pdf)
- [user-manual|MID360用户手册](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Mid360/20230727/Livox_Mid-360_User_Manual_CHS.pdf)
- [Livox_sdk2源地址](https://github.com/Livox-SDK/Livox-SDK2)
- [livox_ros_driver2源地址](https://github.com/Livox-SDK/livox_ros_driver2)
- [livox ros driver2安装博客](https://blog.csdn.net/qq_29912325/article/details/130269367?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169734904416800182711632%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169734904416800182711632&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-2-130269367-null-null.142^v96^pc_search_result_base9&utm_term=livox_sdk2&spm=1018.2226.3001.4187)
- [虚拟机和mid360桥接博客](https://blog.csdn.net/sinat_39110395/article/details/123545816?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169735401816800227447255%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169735401816800227447255&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-123545816-null-null.142^v96^pc_search_result_base9&utm_term=%E8%99%9A%E6%8B%9F%E6%9C%BA%E8%BF%9E%E6%8E%A5%E9%9B%B7%E8%BE%BE&spm=1018.2226.3001.4187)
## Livox和Fast-Lio配置流程
1. 安装[Livox_sdk2](https://github.com/Livox-SDK/Livox-SDK2),readme有写相关过程，注意：要更改主机ip为192.168.1.50[ubuntu修改方法](https://blog.csdn.net/sinat_39110395/article/details/123545816?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169735401816800227447255%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169735401816800227447255&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-123545816-null-null.142^v96^pc_search_result_base9&utm_term=%E8%99%9A%E6%8B%9F%E6%9C%BA%E8%BF%9E%E6%8E%A5%E9%9B%B7%E8%BE%BE&spm=1018.2226.3001.4187)，本人雷达ip为192.168.1.180
2. 安装[livox_ros_driver2](https://github.com/Livox-SDK/livox_ros_driver2), readme有写相关过程， 注意运行前要注意更改config里面的主机ip和雷达IP
3. 配置fast-lio
参考：
[FAST_LIO原地址](https://github.com/hku-mars/FAST_LIO)
[FAST-LIO配置中文博客](https://blog.csdn.net/qq_42108414/article/details/131530293?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169737102216800185825796%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169737102216800185825796&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-131530293-null-null.142^v96^pc_search_result_base9&utm_term=fast%20lio%E9%85%8D%E7%BD%AE&spm=1018.2226.3001.4187)
[关于在ROS1下用MID360配置FAST-LIO2备忘](https://blog.csdn.net/qq_52784762/article/details/132736322?ops_request_misc=&request_id=&biz_id=102&utm_term=fast%20lio%E9%85%8D%E7%BD%AE&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-132736322.142^v96^pc_search_result_base9&spm=1018.2226.3001.4187)
更建议参考源地址的READEME
```bash
sudo apt install libeigen3-dev
sudo apt install libpcl-dev
```
```bash
# ros2需要安装
sudo apt install ros-humble-pcl-ros
```
``` bash
# 编译fast-lio
#这里进入的是你自己的工作空间的src，注意替换$A_ROS_DIR$
cd ~/$A_ROS_DIR$/src
git clone https://github.com/hku-mars/FAST_LIO.git
cd FAST_LIO
git submodule update --init
cd ../..
catkin_make
source devel/setup.bash
# 注意，如果使用的是mid360，即使用的是livox_ros_driver2而非1的话，
# 需要前往fast-lio的CmakeLists文件修改find_package里的livox_ros_driver为livox_ros_driver2，同时package.xml里面的也一样，对应的src中的cpp文件也需要修改，嫌改的麻烦的话mid360可以直接用我仓库中的fast_lio
```
``` bash
# 安装sophus
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ../ -DUSE_BASIC_LOGGING=ON
make
sudo make install
```
上述步骤可能会报错,[解决方案](https://blog.csdn.net/DerrickRose25/article/details/130173310?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169737303816800215088736%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=169737303816800215088736&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-8-130173310-null-null.142^v96^pc_search_result_base9&utm_term=lvalue%20required%20as%20left%20operand%20of%20assignment%20%20%20%20unit_complex_.real%28%29%20%3D%201.%3B&spm=1018.2226.3001.4187)
``` bash
/home/lau/Sophus/sophus/so2.cpp:32:26: error: lvalue required as left operand of assignment
unit_complex_.real() = 1.;
^~
/home/lau/Sophus/sophus/so2.cpp:33:26: error: lvalue required as left operand of assignment
```
打开其位置so2.cpp:32:26改为
``` bash
SO2::SO2()
{
unit_complex_.real(1.);
unit_complex_.imag(0.);
}
```
sophus安装成功后再重新编译fast-lio
最后运行
``` bash
# 进入livox_ros_driver2所在的工作空间
cd ~/$A_ROS_DIR$
source devel/setup.bash
roslaunch livox_ros_driver2 msg_MID360.launch
#再开一个终端，进入fast_lio所在的工作空间
cd ~/$A_ROS_DIR$
source devel/setup.bash
roslaunch fast_lio mapping_mid360.launch
```
以上就是使用fast_lio建好的图的效果，最后会自动存在PCD文件夹中
## 导航流程
一些基础的入门介绍：
- [ROS入门(九)——机器人自动导航](https://blog.csdn.net/Netceor/article/details/118997851?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169779395316800215096913%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169779395316800215096913&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-118997851-null-null.142^v96^pc_search_result_base9&utm_term=ros%E5%AF%BC%E8%88%AA%E6%B5%81%E7%A8%8B&spm=1018.2226.3001.4187)
- [带你理清：ROS机器人导航功能实现、解析、以及参数说明](https://blog.csdn.net/qq_42406643/article/details/118754093)
---
### 1.Relocalization
上面的建图完成后，如果我们希望下次机器人导航的时候，能找到机器人自己的当前位置，那么就需要对机器人进行定位操作。也有如下几种方式：
- 方式一：直接在初始位置使用里程计定位，也就是说，如果能确保机器人每次的上电和程序运行的初始位置始终一至，那么就可以使用fast_lio的里程计进行机器人定位。但这样的劣势很明显：1.机器人需要确保自己每次的初始位置一致，否则就会有误差；2.机器人在移动时，里程计会产生累计误差，无法消除。
- 方式二：使用地图进行重定位，简单来说，就是通过当前雷达的点云和已构建的地图进行匹配来找到机器人当前的位姿。一般二维地图采用amcl进行重定位，三维点云采用icp进行重定位，当然，也有一些新的论文会提出新的更好的重定位定位方法，这些优化的算法暂不讨论。这种重定位方式一般会要求人给一个大概的初始位置，又或者可以依靠机器人的其他传感器给出一个粗略的初始位置估计。
考虑到避免累计误差，我们采用方式二进行定位。我们选用了一个开源的重定位代码(实际采用的也是icp，使用python写的，考虑运行速度的话可以自行使用c++实现或者使用其他icp重定位代码)：[FAST_LIO_LOCALIZATION](https://github.com/davidakhihiero/FAST_LIO_LOCALIZATION-ROS-NOETIC)
```bash
# 所需包
sudo apt install ros-$ROS_DISTRO-ros-numpy
pip install numpy==1.21
pip install open3d
```
目前这套适用python3.8，如果你有多个python环境，自行修改哈。
在配置时，我们发现了一些问题，主要还是版本导致。
一个是原仓库使用的是python2，python2使用和安装外部库已经不太方便了，所以换成了python3。
另一个是FAST_LIO_LOCALIZATION里面也包含了FAST_LIO，但是这里面的FAST_LIO还是比较老的版本，建议还是从最新的FAST_LIO仓库里拉最新的(因为做了mid360的适配)，然后记得在cmakelist里修改生成的可执行文件的名称，否则会和上一步的fast_lio的mapping重复报错。
所以想使用的话建议还是直接使用我们的`sentry_slam/FAST_LIO_LOCALIZATION`和`sentry_slam/FAST_LIO`这两个修改好的包。
同时记录一下我对[FAST_LIO_LOCALIZATION](https://github.com/davidakhihiero/FAST_LIO_LOCALIZATION-ROS-NOETIC)所做的修改，包括以下文件:
- `global_localization.py`
- `#!/usr/bin/python3` 此处我们修改解释器为python3
- `*import _thread*`, python3中使用thread会报错，已经改名为_thread
- 在open3d的最新版本, `o3d.registration`应被替换为 `o3d.pipelines.registration`
- `FOV = 6.28` in 222 line 应该改成你使用的雷达的扫描范围. The scale of MID360 is 360, so 2*pi (rad)
- FOV_FAR = 30, switch to you lidar max distance
- `localization_MID360.launch`
- 我们修改了 `fastlio_mapping` 可执行文件的所属包，我们直接使用fast_lio2中的mid360的launch文件启动
- 使用 `args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" />` in line 28，而不是`/map`,即和你自己的tf树一致
- modified to ``, that used the PCD file in FAST_LIO pkg, If you have your own PCD file, you can change it to your own PCD file path.
Usage:
```bash
roslaunch livox_ros_driver2 msg_MID360.launch
roslaunch fast_lio_localization localization_MID360.launch
# 发布初始位姿(也可以用rviz，第一次尝试的时候更建议使用rviz)
rosrun fast_lio_localization publish_initial_pose.py 0 0 0 0 0 0
# also you could publish your initial point use RVIZ
# 这里的原点是你建图时候的起点。
```
---
### 2. 地图转换（PCD to 二维栅格地图）
这里面红色的点云就是PCD文件显示的三维点云地图，白色的就是熟知的二维栅格地图
地图转换主要是因为move_base是基于2d的栅格地图进行路径规划，而fast_lio默认的输出地图是三维点云的PCD文件，我们需要用一些方法获取2d的栅格地图，有以下几种方式：
1. 用fast_lio构建好PCD地图后，将PCD地图转换为栅格地图
方式一：使用[pcd_package](https://github.com/Hinson-A/pcd2pgm_package)开源功能包，参考[离线将PCD地图转换为pgm栅格地图](https://blog.csdn.net/Draonly/article/details/124537069?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165207936116781435426048%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=165207936116781435426048&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-6-124537069-null-null.142%5Ev9%5Econtrol,157%5Ev4%5Econtrol&utm_term=pcd%E5%9C%B0%E5%9B%BE%E8%BD%AC%E6%8D%A2%E4%B8%BA%E6%A0%85%E6%A0%BC%E5%9C%B0%E5%9B%BE&spm=1018.2226.3001.4187)
方式二：使用`octomap_server`功能包,离线将pcd转换成栅格地图，参考[octomap_server使用－－生成二维占据栅格地图和三维概率地图](https://blog.csdn.net/sru_alo/article/details/85083030?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169804282616800213031883%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169804282616800213031883&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-2-85083030-null-null.142^v96^pc_search_result_base9&utm_term=%E7%82%B9%E4%BA%91%E5%9C%B0%E5%9B%BE%E7%94%9F%E6%88%90%E6%A0%85%E6%A0%BC%E5%9C%B0%E5%9B%BE&spm=1018.2226.3001.4187)
2. 在fast_lio构建三维点云地图的同时，也实时构建2d的栅格地图
本文的代码仓库里两种方式都有，常用的是第二种，如果你是打比赛之类的用的话，建议你建图的时候可以使用rosbag录制一下你的雷达的话题（imu+pcd）,这样就算建图效果一般，也能离线重新rosbag play进行建图
#### 配置：
```bash
sudo apt install ros-noetic-map-server
# 打开一个终端.(ctrl+alt+T)输入下面指令安装octomap.
sudo apt-get install ros-noetic-octomap-ros #安装octomap
sudo apt-get install ros-noetic-octomap-msgs
sudo apt-get install ros-noetic-octomap-server
# 安装octomap 在 rviz 中的插件
sudo apt-get install ros-noetic-octomap-rviz-plugins
# install move_base
sudo apt-get install ros-noetic-move-base
#如果使用方式一，还需将pcd2pgm拉到工作空间的src目录下编译
#本文代码仓库已经包含了该仓库，再sentry_tools/pcd2pgm，如果直接使用本文代码仓库，则不需要再拉
git clone https://github.com/Hinson-A/pcd2pgm_package.git
```
#### 方式一实现
```bash
# pcd2pgm offline
# modify run.launch file in pcd2pgm such as the pcd file pasth etc.
# 修改 pcd2pgm中的run.launch文件，修改输入的pcd文件路径等
roslaunch pcd2pgm run.launch
```
#### 方式二实现
使用`octomap_server`功能包中的`octomap_server_node`节点, 实时读取三维点云, 并生成栅格地图(这个方法其实也能离线将PCD转为栅格地图).
我们在 `FAST_LIO` 功能包中添加了 `Pointcloud2Map.launch`, which will update the 2D mapping at same time, if you publish the PointCloud2 from FAST_LIO.
然后我们综合了 `SLAM`, `relocalization`, `实时构建栅格地图`三个功能, in only one launch file ==> `sentry_build.launch`.
运行此功能包后，系统便会开始自动同步构建栅格地图.
如果你对你构建的三维点云地图和二维栅格地图满意，并希望保存下来：
1. 三维点云地图pcd文件会在`sentry_build.launch`运行结束后自动保存到fast_lio/PCD文件夹下
2. 如果你希望保存二位栅格地图，请运行以下命令：
```bash
# save the pgm map file
rosrun map_server map_saver map:=/ 
