# navigation_system_ws

**Repository Path**: CeasarSmj/navigation_system_ws

## Basic Information

- **Project Name**: navigation_system_ws
- **Description**: No description available
- **Primary Language**: C++
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 10
- **Forks**: 4
- **Created**: 2025-06-17
- **Last Updated**: 2026-01-16

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 导航系统
这是基于 SAS 2024-2025 年的导航系统老代码翻新和整理出的项目，添加了一部分功能，并整理了代码使其更加整洁。

老代码参见：https://gitee.com/ucas-sas-robot-team/ros-navigation-system

这套代码中只包含 SLAM+导航部分，而串口通信部分由于原项目中已经比较完善将不再重复处理；决策部分需要晚些重构所以也不会添加。


## 环境配置
- Ubuntu20.04
- ros noetic

具体的库文件参考 src 下各个子目录中包的配置方式。

## 讲解
### 1. 快速启动
参照以下步骤，你可以快速启动库并启动一个测试例程，看到导航的效果：

- 配置和编译
1. 下载包，在命令行执行 ```git clone https://gitee.com/CeasarSmj/navigation_system_ws.git```。
2. 重命名 fast_lio_exp 和 livox_ros_driver2 目录，在名字前面加一个'.'，这样它们就不会影响其它包的编译。
3. 参考 src 下子目录 map_trans_toolkit_pkg 和 navigation_exp_pkg 的 README.md 文件，编译并安装相关包。
4. shell 打开到工作空间目录下，单独编译这两个包，执行 ```catkin_make ```。
5. 如果提示缺少依赖包，请执行 ```sudo apt install <依赖包名>```，一般会是```lib<xxx>-dev```或者是```ros-noetic-xxx```，建议搜一下。

- 准备地图文件
1. 编译完成后 shell 打开到 src/map_trans_toolkit_pkg/maps/cad，执行 ```python3 stl2pcd.py```。（ps:这里只上传了stl文件再转化是为了省空间）
2. 可以观察到 maps 目录下多出了一个 map.pcd，这是我们后续用于地图处理的点云文件，可以试着用 vscode 插件pcd-viewer 加载看一下。
3. shell 打开到工作空间目录下，加载 ros1 环境(```source /opt/ros/noetic/setup.bash```)和 工作空间环境 (```source devel/setup.bash```)
4. 执行 ```roslaunch map_trans_toolkit_pkg pcd_to_pgm.launch```,输出```data size =``` 时就成功了，可以用 rviz 看一下，或者直接 ctrl+c 退出。

- 启动导航
1.  执行```roslaunch navigation_exp_pkg simulation_movebase.launch```，启动导航。
2.  rviz 出现后，使用菜单栏紫红色箭头"2D Nav Goal"点击地图上某处，可以观察到TF座标架正在向指定位置移动。也可以使用绿色箭头进行重定位，手动指定位置。


如果要使用节点图版本的导航，可以继续下面的步骤：
- 地图准备
1. 用 GIMP 打开地图文件 src/map_trans_toolkit_pkg/maps/map.pgm （GIMP 可以直接用 apt 指令安装）
2. 手动编辑处理一下地图，封闭不可达区域，完成后保存为 map.pgm
3. 用 shell 打开到地图转化脚本目录 src/map_trans_toolkit_pkg/scripts_graph ，打开脚本 `get_graph_from_image.py`
4. 设置 `mode= MODE[0]` 并执行脚本生成节点图文件 `python3 get_graph_from_image.py`
5. 设置 `mode= MODE[2]` 并执行脚本生成节点图文件 `python3 get_graph_from_image.py`
5. 启动导航，执行 `roslaunch navigation_exp_pkg simulation_graph.launch`

完成上述步骤，你就完成了这个导航系统的启动。
如果需要部署到实机上，请参考后面的步骤。

### 2. Livox Mid360 以及它的 配套工具
Livox Mid360 是一个3D激光测距传感器，用于测量激光距离。
它可以视为一种扫描仪，能够扫描和记录出周围环境的 3D 点云地图。

想要熟练的使用它，需要学会使用以下：
1. 连线：
   - 使用3合一航空转接线，航空头连接雷达，另一头网口可以直接连电脑，电源线供电 12-18V
2. 配置修改工具-- livox_viewer ：
   - 直接上官网下载自己需要的版本(https://www.livoxtech.com/cn/mid-360/downloads)，然后启动
   - 连接上雷达后点击设置(工具栏齿轮图标)，设置或者记录它的 ip 地址
4. 驱动 Livox SDK:
   - 去 github 上搜一下，直接安装即可
5. ros 包 livox_ros_driver2:
   - 直接安装即可，在 src/livox_ros_driver2_/README.md 中有说明

### 3. fast_lio SLAM 建图与重定位
- 简述
fast_lio 是一个 3D 地图构建与定位工具，原项目可以参考 [fast_lio](https://github.com/hku-mars/fast_lio)。

- 建图

它可以提供实时定位和建图功能，但是需要完成和其它模块的对接。
我们使用的是 Livox_Mid 360 雷达，如果需要使用其它雷达，可以参考原项目中的说明来修改 launch 和 config 文件。

fast_lio 的参数配置在```config/mid360.yaml```中，可以自行修改。
主要修改点是话题名。

建图启动的 launch 文件为```launch/mapping_mid360.launch```，使用时可以在其它包中<include>它并设置参数。


- 重定位

但是仅仅有建图并不足够，如果要执行导航任务，那么我们需要依赖固定坐标值，那么需要保证我们对自己的定位和我们的先验地图是对齐的，所以需要进行重定位。

在 fast_lio SLAM 过程中有一个坐标系统，也就是 camera_init->body ，其中 camera_init 表示 SLAM 建图第一帧时机器人（或者说雷达）所在的位置。
而要对齐 SLAM 地图和先验地图，那么就可以加一个坐标系统 map 作为先验地图（其实是先验pcd开始扫描的第一帧的坐标系），变成 map->camera_init->body，这里camera_init 我们可以认为起了一个里程计的作用。

而执行这个重定位任务的节点是 fast_lio_exp/scripts/global_localization.py 或者 fast_lio_exp/scripts/global_localization2.py 或者 fast_lio_exp/src/localization.cpp。

另外重定位启动依赖于初始化位置和初始先验地图。
初始位置参见 fast_lio_exp/scripts/publish_initial_pose.py ，里面的值可以修改成自己需要的。它的作用等效于 rviz 中的设置初始位置。
初始先验地图参见 fast_lio_exp/launch/localization.launch 中的 pcd_to_pointcloud 参数。
不过的 fast_lio_exp/scripts/global_localization2.py 或者 fast_lio_exp/src/localization.cpp 是经过了重构的，所以可以重复赋值进行热更新。


### 4. map_trans_toolkit_pkg 地图转换工具包
这个包里的工具脚本很多，具体可以参见包中的 README.md 文件，这里只写主要的。
1. 大型 PCD 文件发布：
   - 参考了游戏中地图的加载方式，分块加载，靠近加载，原理删除
   - 可以预先把 pcd 分割成多个小的 pcd 文件，然后用程序动态加载机器人周围指定距离内的 pcd 并发布
2. pcd 转 pgm:
   - 这个在快速启动中已经用到了，可以把先验 pcd 截取指定高度变成 pgm 地图
   - 根据需要调节参数，文件在 map_trans_toolkit_pkg/launch/pcd_to_pgm.launch
3. 高程图处理：
   - 对于可以爬坡的机器人，截取高度的简单操作不足以实现地图获取
   - 所以可以从 pcd 获取一定区域内的搞成图，参见 map_trans_toolkit_pkg/launch/pc_to_elevation.launch
   - 同时 move_base 平面导航需要用的其实是 laserscan 作为实时地图，所以也有对应的转化节点 elevation_to_map.launch 和 ocpg_to_laserscan.launch
   - 高程图转平面图时会测量相邻位置之间的高度差，如果高度差小于阈值则视为可以通过，从而可以实现对于斜坡的处理
4. 离散化地图处理：
   - 对于复杂的空间结构，平面地图并不能成功表示，例如立交桥。而用无向图结构则可以表示任何结构
   - 所以我们可以通过点云处理来获取无向图架构，参见 map_trans_toolkit_pkg/scripts_pcdtrns/pcd_to_graph.py
   - 为了通用性，也提供了一些对于平面图直接进行离散化的工具，参见 map_trans_toolkit_pkg/scripts_graph/get_graph_from_image.py

### 5. navigation_exp_pkg
这是一个导航包，里面封装了两种导航：
- 基于图结构进行全局路径规划 + 基于 teb_local_planner 进行局部路径规划
- 直接使用 move_base 进行导航

其中二者会共用 局部代价地图参数 和 teb 局部规划期参数。

关于调参一部分可以参考配置中的注释来进行调整，也可以上网搜索用法。


### 6. simulation_pkg
环境 ros noetic ， 编写一个包 simulation_pkg 来在空间中实现机器人移动。
使用 gazebo 作为模拟环境。
文件结构：
```
simulation_pkg
|- config
   |- controller.yaml
|- worlds
   |- empty.world
|- mesh
|- launch
   |- simulation.launch
|- models
   |- robot.xacro
|- rviz_config
   |- navigation.rviz
```

机器人为四轮差速，包含了 4 个电机；包括一个 IMU , 一个 3D 激光雷达，一个 相机。
其中 IMU 发布到话题 /imu， 激光雷达发布到话题 /lidar_scan， 相机发布到话题 /camera/image_raw。

### 关于实机部署
- 本导航项目已经相对完善了，只需要添加决策和串口通信部分。

- 决策部分需要负责指定要去哪个位置并发送到 /move_base_simple/goal 话题（或者使用 action 服务接口），可以用有限状态机或者行为树实现。

- 从 /cmd_vel 到 串口通信的编码代码需要自行编写，可以参考这个包的写法：https://gitee.com/ucas-sas-robot-team/ros-navigation-system/tree/master/src/middle_coding_pkg

- 串口通信部分如果使用 CAN 可以参考：https://gitee.com/ucas-sas-robot-team/ros-navigation-system/tree/master/src/usb_can_pkg，
或者使用其它现成的通信包。


准备好这些软件部分的工作后，还需要完成先验地图扫描（或者从 CAD 文件转化）和预处理的工作（参考快速启动）。
完成这些步骤后，机器人就可以由导航系统驱动着动起来了。