ROS开发专栏---基于 NAV2 实现仿真环境自主导航实验--适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ： 《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ： 《freertos专栏》 《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》 《Ai智能体专栏） 《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

​

目录

前言

一、NAV2（Navigation2）的基本概念

[1.1、Map Server 地图服务器](#1.1、Map Server 地图服务器)

[1.2、Planner 全局路径规划器](#1.2、Planner 全局路径规划器)

[1.3、Smoother 路径平滑器](#1.3、Smoother 路径平滑器)

[1.4、AMCL 定位器](#1.4、AMCL 定位器)

[1.5、Controller 运动控制器](#1.5、Controller 运动控制器)

[1.6、Recovery 导航恢复行为](#1.6、Recovery 导航恢复行为)

[1.7、Velocity Smoother 速度平滑器](#1.7、Velocity Smoother 速度平滑器)

[1.8、BT Navigator 行为树管理器](#1.8、BT Navigator 行为树管理器)

[1.9、ROS2 NAV2与ROS1 move_base区别](#1.9、ROS2 NAV2与ROS1 move_base区别)

二、NAV2的安装

三、使用NAV2实现自主导航

3.1、导入地图文件

[3.2、创建 NAV2 导航功能包](#3.2、创建 NAV2 导航功能包)

[3.3、编写 NAV2 导航启动 Launch 文件](#3.3、编写 NAV2 导航启动 Launch 文件)

[3.4、CMakeLists.txt 配置说明及完整代码](#3.4、CMakeLists.txt 配置说明及完整代码)

[3.5、package.xml 配置说明及完整代码](#3.5、package.xml 配置说明及完整代码)

3.6、编译功能包

3.7、仿真环境自主导航完整运行流程

[3.7.1、启动 Gazebo 仿真环境](#3.7.1、启动 Gazebo 仿真环境)

[3.7.2、启动 NAV2 导航 + RViz2 可视化](#3.7.2、启动 NAV2 导航 + RViz2 可视化)

[3.7.3、RViz2 手动设置定位与导航目标](#3.7.3、RViz2 手动设置定位与导航目标)

3.7.4、指定导航目标点

总结

---

前言

上一期博客我们用 SLAM Toolbox 完成了仿真环境的激光建图，生成了 map.pgm 和 map.yaml 栅格地图文件。

本期博客我们基于这张地图，使用 ROS2 官方导航栈 NAV2 实现机器人自主导航。

NAV2 是 ROS2 时代的标准导航框架，替代了 ROS1 的 move_base，支持路径规划、避障、定点巡航、多目标点导航等功能，是机器人自主导航的核心工具。

---

一、NAV2（Navigation2）的基本概念

首先我们来看一下现实中人是怎么进行导航的，如下图所示：

ros2中也有一整套模块化导航系统，官方架构流程图如下图所示：

包括多个功能节点，总共有8个核心组件，具体我们可以按如下图所示：

1.1、Map Server 地图服务器

导航需要地图作为路径规划的依据，这个地图数据由Map Server地图服务器提供，发布 /map 地图数据，供其他节点订阅获取。

1.2、Planner 全局路径规划器

此节点主要负责生成全局导航路径。它先从地图服务器的"/map"话题获取全局地图，再从机器人的激光雷达话题获取雷达测距的障碍物点云。将两者叠加后，生成避障用的全局代价地图（在全局路径规划时算一次）。

在全局代价地图里，障碍物的边缘会膨胀出一层半透明的渐变区域。

这个代表的就是机器人可能与障碍物发生碰撞的隐性"代价"。越靠近障碍物，与障碍物碰撞的风险越大，于是颜色越鲜艳，隐性"代价"越大。如下图所示：

生成了代价地图之后，路径规划器Planner使用A*和Dijkstra's（默认）这类路径规划算法生成一条连接起始点和目标点的路径曲线 ，这就是规划出来的全局导航路径。如下图所示：

1.3、Smoother 路径平滑器

因为Planner 全局路径规划器生成的全局路径可能是会有急转弯、锐角转折等不符合实际的路线，所以需要路径平滑器对路线进行优化

1.4、AMCL 定位器

有了导航路径之后，还需要机器人知道自己在地图中的位置。而**AMCL 定位器就是进行机器人的实时自我定位，**这是一种基于概率统计的粒子滤波算法。绿色的这些小箭头就是粒子，如下图所示：

1.5、Controller 运动控制器

运动控制器会从路径平滑器Smoother获取优化后的最终路径曲线，然后根据AMCL定位器给出的机器人定位信息，规划出机器人当前位置的运动策略，尽量贴合这条路径曲线。并按照策略计算出机器人的运动速度，然后通过话题发送速度消息包给速度平滑器。最终由速度平滑器来控制机器人进行运动。

controller_server 同时承担了 "局部规划 + 轨迹跟踪 + 动态避障" 的所有工作。

注意：局部路径规划与动态避障默认采用 DWB 控制器（DWA 算法）

1.6、Recovery 导航恢复行为

其作用是让机器人从极端情况下脱离险境，自动执行后退、原地转圈、重试等脱困恢复行为。这些行为也是通过话题发送速度消息包给速度平滑器，由速度平滑器来驱动机器人进行移动。如下图所示：

图中状态	NAV2 对应恢复行为	作用说明
正常导航	-	机器人按规划路径正常行驶，无异常
保守重置	BackUp（后退）	机器人向后退一小段，尝试脱离局部障碍
旋转清除	Spin（原地旋转）	机器人原地 360° 旋转，清除代价地图里的错误障碍标记
激进重置	ClearCostmap（清代价地图）	直接重置全局 / 局部代价地图，重新感知环境
放弃任务	任务失败回调	所有恢复行为都失败后，停止导航并报告任务失败

1.7、Velocity Smoother 速度平滑器

速度平滑器的作用是将上游运动控制器发送来的速度进行平滑处理，避免出现控制数值的突变，尽量保护硬件设备的安全运行

1.8、BT Navigator 行为树管理器

之前的图中所描述的处理流程是通过一种名为Behavior Tree行为树的形式来组织的，统一调度、编排、管控整个导航全流程，可灵活自定义导航逻辑。

**1.9、**ROS2 NAV2与ROS1 move_base区别

对比维度	ROS1 move_base	ROS2 NAV2
架构设计	单节点一体化	多节点模块化分布式
决策方式	固定硬编码状态机	行为树 BT 可自由配置流程
通信机制	ROS1 actionlib 不稳定	ROS2 DDS + Action 可靠稳定
可扩展性	差，改逻辑需改源码	极强，全插件化即插即用
传感器适配	仅 2D 激光为主	原生支持 2D/3D 多传感器
调试难度	高，仅靠日志排查	低，RViz2 全流程可视化
维护状态	停止维护、已淘汰	ROS2 官方主推，长期迭代

二、NAV2的安装

要在ROS2中使用NAV2，需要先安装相应的软件包，在终端中输入以下指令：

sudo apt install ros-humble-navigation2

ROS2官方建议安装一个NAV2的Bringup软件包，借助这个软件包的Launch文件来启动NAV2。安装指令如下所示：

 sudo apt install ros-humble-nav2-bringup

三、使用NAV2实现自主导航

3.1、导入地图文件

上一期博客建图完成后，终端会生成 map.pgm 和 map.yaml 两个文件。为了方便后续管理和加载，我们把它们放到 wpr_simulation2 包的 maps 目录下。如下图所示：

3.2、创建 NAV2 导航功能包

进入工作空间 src 目录，创建导航功能包 nav_pkg：

cd ~/ros2_zice/src
ros2 pkg create nav_pkg

在软件包中创建一个Launch文件，命名为"launch"。在文件夹新建文件，命名：nav.launch.py。

3.3、编写 NAV2 导航启动 Launch 文件

在进行 NAV2 自主导航之前，我们需要编写一个启动文件: nav.launch.py，用来一键加载地图、启动 NAV2 所有导航节点、打开 RViz 可视化界面。

这个 launch 文件的核心作用有 4 个：

指定地图路径：告诉 NAV2 从 wpr_simulation2/maps 加载我们建好的地图。

加载导航参数：读取仿真包自带的导航配置文件，不用自己从零写。

启动官方 NAV2：直接调用 ROS2 自带的 bringup_launch.py，一键启动全套导航。

打开 RViz：自动加载导航专用配置，方便我们设置目标点、观察路径。

完整代码如下所示：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # 加载存放在wpr_simulation2/maps下的地图配置文件
    map_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'maps',
        'map.yaml'
    )

    # 加载NAV2定位、规划、控制参数文件
    nav_param_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'config',
        'nav2_params.yaml'
    )

    # 获取nav2_bringup官方启动文件目录
    nav2_launch_dir = os.path.join(
        get_package_share_directory('nav2_bringup'), 
        'launch'
    )

    # 调用官方NAV2启动文件，传入地图、仿真时间、参数配置
    navigation_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([nav2_launch_dir, '/bringup_launch.py']),
        launch_arguments={
            'map': map_file,
            'use_sim_time': 'True',
            'params_file': nav_param_file}.items(),
    )

    # 加载导航专用RViz配置
    rviz_file = os.path.join(
        get_package_share_directory('wpr_simulation2'), 
        'rviz', 
        'navi.rviz'
    )

    # 启动RViz2可视化界面
    rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_file]
    )

    # 整合所有启动节点
    ld = LaunchDescription()
    ld.add_action(navigation_cmd)
    ld.add_action(rviz_cmd)

    return ld

3.4、CMakeLists.txt 配置说明及完整代码

CMakeLists.txt 是 ROS2 功能包编译配置文件，作用是声明编译版本、加载项目依赖、配置文件安装规则。因为本功能包用到 NAV2、rclcpp 等相关模块，必须逐一声明依赖。全部代码如下所示：

cmake_minimum_required(VERSION 3.8)
project(nav_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 加载所需依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(nav2_bringup REQUIRED)
find_package(navigation2 REQUIRED)

# 安装launch文件夹，让ROS2能识别启动文件
install(
  DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}
)

# 测试相关默认配置
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

3.5、package.xml 配置说明及完整代码

package.xml 是 ROS2 功能包依赖声明文件，用来告诉 ROS2 系统当前包依赖哪些官方功能包。凡是 launch 文件中用到的 NAV2、rclcpp 等模块，都必须用 <depend> 标签声明，缺少任意一个都会导致编译报错或节点运行异常。

完整代码如下所示：

<?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>nav_pkg</name>
  <version>0.0.0</version>
  <description>NAV2仿真环境自主导航功能包</description>
  <maintainer email="todo@todo.todo">todo</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <!-- 项目所需全部依赖 -->
  <depend>rclcpp</depend>
  <depend>nav2_bringup</depend>
  <depend>navigation2</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

3.6、编译功能包

所有配置文件编写完成后，回到工作空间编译并刷新环境变量，打开终端，输入以下指令：

cd ~/ros2_zice
colcon build
source install/setup.bash

3.7、仿真环境自主导航完整运行流程

3.7.1、启动 Gazebo 仿真环境

打开终端，输入以下指令：

source install/setup.bash
ros2 launch wpr_simulation2 robocup_home.launch.py

如下图所示：

3.7.2、启动 NAV2 导航 + RViz2 可视化

新建终端，输入以下命令：

作用：自动加载地图、启动全套 NAV2 导航节点、打开 RViz2 可视化界面。

source install/setup.bash
ros2 launch nav_pkg nav.launch.py

如下图所示：

3.7.3、RViz2 手动设置定位与导航目标

点击 RViz 工具栏 2D Pose Estimate；

在地图上机器人真实位置按住鼠标左键拖动；

箭头指向为机器人初始朝向；

松开鼠标后出现粒子云，代表 AMCL 开始匹配定位。

设置好如下所示：

3.7.4、指定导航目标点

点击 RViz 工具栏 Nav2 Goal。

在地图空旷位置点击并拖动，设置目标位置与朝向。

松开后 NAV2 自动生成紫色全局路径。

机器人自动沿路径避障行驶，到达终点后自动对齐目标朝向。

设置好之后，全局规划器会自动规划出一条紫色的路径。这条路径从机器人当前点出发，避开障碍物，一直到导航目标点结束。如下图所示（下图已经是走了一小段时间了）：

可以看到此时Gazebo仿真界面上，机器人也同步运动了，如下图所示：

机器人到终点后，会原地旋转，调整航向角，最终朝向刚才设置目标点时绿色箭头的方向。如下图所示：

---

总结

本期博客我们使用 ROS2 官方导航栈 NAV2 实现了机器人自主导航。