## Navigation2多点导航避障

### 1、程序功能说明

**注意：**学习本节课程需要先学习【Navigation2单点导航避障】，对Navigation2导航有基本认识
运行程序后，rviz中会加载地图。在rviz界面中，使用nav2插件进行多点导航
### 2、运行案例
#### 2.1 使用前说明
直接打开终端，输入本节课程提及的指令即可。
#### 2.2 多点导航
**注意：**
*   本章节课程需要至少已经存在一张已经建好的地图，参考**【gmapping建图】、【cartographer建图】、【slam_toolx建图】**任意一节SLAM建图课程
机器人车机终端启动底层传感器命令：
```
ros2 launch yahboomcar_nav laser_bringup_launch.py
```
![](/media/202512/341d973ae2c54b0db91227381f65f4732594.png)
输入指令启动rviz可视化建图 （rviz可视化功能在车机端或虚拟机端启动都可以，**任选一种**方式启动，禁止虚拟机端和车机端重复启动：），下图是在车机端输入，
```
ros2 launch yahboomcar_nav display_nav_launch.py
```
![](/media/202512/c528ad868c034320bab35d17b4f03a6a1589.png)
此时还没有显示地图加载，因为还没有启动导航的程序，所以没有地图加载。接下来运行导航节点，终端输入

#导航二选一
#teb

```
ros2 launch yahboomcar_nav navigation_teb_launch.py 
```

#dwa

```
ros2 launch yahboomcar_nav navigation_dwa_launch.py
```

![](/media/202512/bb13f3dc67824c6da6bbac4f63f900039196.png)

此时可以看到地图加载进去了，然后我们点击【2D Pose Estimate】，给小车设置初始位姿，根据小车在实际环境中的位置，在rviz中用鼠标点击拖动，小车模型移动我们设置的位置。如下图所示，雷达扫描的区域与实际障碍物大致重合则表示位姿准确。位姿初始化完成后，rviz界面中会出现机器人模型和膨胀区域

![image-20250813114825131](/media/202512/ad9872f4cbd248d89af021b99b0ae61d2103.png)

点击rviz左上角【panels】把其中的Navigation2插件添加进来

![image-20250813115027352](/media/202512/8488c498a9aa41b1acefb321df89013f2775.png)

添加后，rviz显示如下，

![image-20250813115006621](/media/202512/30ca44308ed44ff596f3d76abfcf49755144.png)

然后点击【Waypoint/Nav Through Poses Mode】,

![image-20250813115112753](/media/202512/8f71594648bb475ea267653d9124054e2603.png)

使用rivz工具栏中的【Nav2 Goal】给定任意的目标点 ，然后点击【Start Waypoint Following】开始规划路径导航。小车会根据选的点的先后顺序，到了目标点后会自动前往下一个点，无需进行操作。达到最后一个点后，小车停车等待下一个指令。

![image-20250813115218203](/media/202512/d129c3e9c72d4c348af054cf7bbc7b582857.png)

三个点都导航成功，终端会显示【Completed all 3 waypoints requested】

![image-20250813115346127](/media/202512/05d352fec2e943f59cf010508653a6269321.png)

### 3、原理解析

#### 3.1 路点数据

用户打开**[Waypoint/Nav Through PoseMode]**,进入多点导航模式后，用户标记的点位信息会被发布到**/waypoints**话题上（rviz路点导航插件会在目标路点之间的路径上额外添加一些路点用来平滑路径），我们可以通过rqt界面来查看这些**路点数据**

终端启动命令：
`
ros2 run rqt_topic rqt_topic
`

我们在rqt界面中可以看到有**/waypoints**话题勾选之后可以观察到话题上的数据（需要先勾选话题，然后在rviz界面发布路点），在rviz中我们手动标记的路点会被发布到这个话题上

![image-20250813120536188](/media/202512/8e318a81f83a4d00a844a562326a569f6007.png)

点开其中一个路点可以查看路点数据，这里以[0]为例，其中pose为坐标数据

![image-20250813120627243](/media/202512/221a8c67f6694b70a65b1faba4d757392728.png)

#### 3.2 数据发送执行

当设置完路点坐标之后点击**Start Waypoint Following**时，rviz插件会将路点坐标序列包装成`FollowWaypoints`动作请求发送给/follow_waypoint动作服务器依次执行所有的路标点

终端输入以下命令:
`
ros2 run rqt_graph rqt_graph
`

在节点关系图中可以看到**/follow_waypoint**动作服务器,该动作服务器接收路点序列然后依次导航。

![](/media/202512/4dcd9fb41a4b4bb694e5eeff354fc5255931.png)