构建导航地图并且使用navigation2进行导航

前言

在机器人仿真中使用ros2_control-CSDN博客（这里放一下上一篇文章的链接）

        上一篇文章介绍了如何将gazebo接入ros2_control，并且配置参数添加控制器，本文章主要介绍如何构建导航地图并且使用navigation2进行导航（这里说一下之前也一直没有提过，如果是初学的话建议从第一篇文章--机器人建模开始看，因为文章中提到的一些文件\代码是在前面几篇文章就已经编写好的，如果从这篇文章开始看的话可能不太容易理解。我的学习还在入门阶段，写这些文章主要是想要回顾总结，写的不好不对的地方欢迎指正）。

一、构建地图

我们构建地图需要使用slam_toolbox，先来安装slam_toolbox，在终端中输入命令

sudo apt install ros-$ROS_DISTRO-slam-toolbox

 先来介绍一下slam_toolbox，它是ros\ros2中最强大的slam（simultaneous localzition and mapping 同时定位与地图的构建）开源解决方案之一，有兴趣的同学可以详细了解一下，我们这里要使用它来构建地图。

接下来启动launch文件，运行gazebo仿真

ros2 launch robot_sim robot1_gazebo_sim.launch.py

仿真启动之后，打开一个新的终端输入命令来启动slam_toolbox 启用仿真时间

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True

然后再打开一个终端，输入rviz2启动rviz2并且点击Add 添加话题下的map、RobotModel、TF

 添加完map之后可以看到类似于这样的画面

这里你可能会看到rviz2报错

 

 它可能会导致可能导致部分可视化元素的纹理显示异常（如地图颜色错乱），但不会破坏数据本身，这个错误‌通常不影响RViz2的核心功能‌，地图可以正常生成，所以我们不管它（我试过很多种办法都没有解决，所以还是不要管它的好）。

再打开一个终端输入命令启动键盘控制节点控制机器人移动

ros2 run teleop_twist_keyboard teleop_twist_keyboard 

这里控制机器人移动前要注意选择FixedFrame为odom，启动键盘控制节点后先按x和c键降低机器人的速度（线速度角速度0.3左右即可），速度太快了建图可能会出问题。

接下来控制机器人在房间内逛一圈，完成扫描的地图大概是这样

对应gazebo中的世界模型

 为了更好的测试导航效果，你们也可以在地图中添加一些物体，我这里添加了书架、桌子、圆柱体、球体和轮胎等物体。

地图中的三种颜色表示三种状态：白色没有没有障碍物--自由空间，黑色有障碍物--已被占用，灰色未知区域\未探索。探索完成不要关闭仿真，因为此时地图还没有保存，要先保存地图在关闭。在src路径下打开终端输入

ros2 pkg create robot1_navigation2

 创建了一个新的功能包，使用code打开src文件，在robot1_navigation2包的根目录下创建一个maps文件夹。接下来我们需要安装一个功能包用于保存地图，在终端中输入

sudo apt install ros-$ROS_DISTRO-nav2-map-server

 安装完这个功能包在maps路径下打开终端输入

ros2 run nav2_map_server map_saver_cli -f room

 这个命令可以启动map_server_cli节点将地图命名为room保存到maps文件夹下保存为一个room.pgm\room.yaml的文件，一个是灰度图片格式只有黑白灰三种颜色，之前也提到了使用黑白灰三种颜色来表示地图的不同状态。

.yaml文件中保存了地图的相关参数

room.pgm是文件名称，trinary表示三色模式，resolution:0.05表示一像素表示0.005米，origin表示原点的位置，negate表是否反转色素值，0表示不反转，0.65表示是否占用的阈值，即0.65到1之间的值代表空间被占用，0.25表示自由空间的阈值，0到0.25表示自由空间，那么0.25到0.65之间就表示未知或者未探索的空间。

二、使用navigation2进行导航

先来介绍一下navigation2，可以将其看做一个处理信息做出决策的装置，它需要传感器数据、实时TF数据、目标点位、地图以及行为树插件作为输入，它的输出只有一个就是机器人的线速度和角速度。行为树可以决定机器人什么时候执行什么动作，这里放的图片是从我学习的up主视频里截取的，我的介绍比较简短，详细的可以去看鱼香ros的视频（视频链接：《ROS 2机器人开发从入门到实践》7.3.1Navigation2介绍与安装_哔哩哔哩_bilibili）

       接下来我们来安装navigation2，在终端输入

sudo apt install ros-$ROS_DISTRO-navigation2

安装 启动示例功能包

sudo apt install ros-$ROS_DISDRO-nav2-bringup

在robot1_navigation2目录下打开终端输入

cp /opt/ros/$ROS_DISTRO/share/nav2_bringup/params/nav2_params.yaml src/robot1_navigation2/config

这个指令会将nav2_bringup功能包中的参数配置文件复制到robot1_navigation2的config目录下

一共有350行有点多，其中包含了navigation2的各个组件的参数配置：比如 全局代价地图、局部代价地图以及规划器、执行器等的配置。这里需要修改一下local_costmap（局部代价地图）和global_costmap（全局代价地图）下的robot_radius(机器人的半径)修改为和自己设置的base_link半径保持一致，

还需要注意参数配置文件中这些默认的坐标系名字是否与自己的一致，我们之前文章中设置的坐标系是与默认的相同的，这里为了以防万一是需要检查一下如：map\base_link\odom\base_footprint等坐标系名称是否对应。 

 

接下来编写launch配置文件，在robot1_navigation2功能包下创建一个launch文件夹并且在文件夹下创建navigation2.launch.py文件在文件中编写

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    #获取所需的路径
    robot1_navigation2_dir=get_package_share_directory('robot1_navigation2')
    nav2_bringup_dir=get_package_share_directory('nav2_bringup')
    rviz_config_dir=os.path.join(nav2_bringup_dir,'rviz','nav2_default_view.rviz')

    #创建launch配置
    use_sim_time=launch.substitutions.LaunchConfiguration(
        'use_sim_time',default='True'
    )
    map_path=launch.substitutions.LaunchConfiguration(
        'map',default=os.path.join(robot1_navigation2_dir,'maps','room.yaml')
    )
    params_path=launch.substitutions.LaunchConfiguration(
        'params_file',default=os.path.join(robot1_navigation2_dir,'config','nav2_params.yaml')
    )

    return launch.LaunchDescription([
        #声明启动参数
        launch.actions.DeclareLaunchArgument('use_sim_time',default_value=use_sim_time,description='use gazebo simulation clock if try'),
        launch.actions.DeclareLaunchArgument('map',default_value=map_path,description='Full of map path to load'),
        launch.actions.DeclareLaunchArgument('params_file',default_value=params_path,description='Full of params path to load'),
        #用当前的launch文件启动示例启动文件
        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir,'/launch','/bringup_launch.py']
            ),
            launch_arguments={
                'map':map_path,
                'use_sim_time':use_sim_time,
                'params_file':params_path,}.items(),
        ),
        #启动rviz2并且传入参数
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d',rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen',
        )])

下面逐一解释代码的含义

    #获取所需的路径
    robot1_navigation2_dir=get_package_share_directory('robot1_navigation2')
    nav2_bringup_dir=get_package_share_directory('nav2_bringup')
    rviz_config_dir=os.path.join(nav2_bringup_dir,'rviz','nav2_default_view.rviz')

拼接路径，get_package_share_directory函数的功能是找到install/share的指定文件，这里找到share目录下的robot1_navigation2和nav2_bringup功能包，使用os拼接nav2_bringup功能包下rviz文件下的rviz默认配置文件路径。

    #创建launch配置
    use_sim_time=launch.substitutions.LaunchConfiguration(
        'use_sim_time',default='True'
    )
    map_path=launch.substitutions.LaunchConfiguration(
        'map',default=os.path.join(robot1_navigation2_dir,'maps','room.yaml')
    )
    params_path=launch.substitutions.LaunchConfiguration(
        'params_file',default=os.path.join(robot1_navigation2_dir,'config','nav2_params.yaml')
    )

声明了三个启动配置参数，use_sim_time是否使用仿真时间，True使用仿真时间，地图.yaml的路径，以及navigation2的参数配置文件路径。

          #声明启动参数
        launch.actions.DeclareLaunchArgument('use_sim_time',default_value=use_sim_time,description='use gazebo simulation clock if try'),
        launch.actions.DeclareLaunchArgument('map',default_value=map_path,description='Full of map path to load'),
        launch.actions.DeclareLaunchArgument('params_file',default_value=params_path,description='Full of params path to load'),

 将声明的启动配置参数重新声明一次，两次声明参数的含义不相同，第一次声明是声明了参数容器--在launch内部传递的参数，表示后续可以调用这些参数，第二次声明是将参数注册到ROS系统中，使得参数能够被外部（如命令行或上层launch文件）识别和修改。

        #用当前的launch文件启动示例启动文件
        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir,'/launch','/bringup_launch.py']
            ),
            launch_arguments={
                'map':map_path,
                'use_sim_time':use_sim_time,
                'params_file':params_path,}.items(),
        ),

在launch中引入官方提供的启动示例bringup_launch.py，并且传入之前声明的启动参数map_path,use_sim_time,params_path。

        #启动rviz2并且传入参数
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d',rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen',

声明启动节点的Action类启动rviz2,并且传入nav2_bringup包下的rviz默认配置文件，使用仿真时间，启动结果输出到屏幕上。

编写完成后在CMakeList文件中添加以下代码

install( DIRECTORY launch config maps
        DESTINATION share/${PROJECT_NAME}
)

将config\maps\launch文件拷贝到install\share\robot1_navigation2目录下，添加完成后保存并且在工作空间下构建代码，构建完成后source install/setup.bash配置一下环境，之后在终端中输入

ros2 launch robot_sim robot1_gazebo_sim.launch.py

先启动仿真，在启动输入

ros2 launch robot1_navigation2 navigation2.launch.py

 启动navigation2和rviz2

启动之后显示的大概是这样一个画面，只是将地图加载进来，还没有加载全局代价地图以及局部代价地图。需要先初始化机器人的位姿，就是需要告诉navigation2机器人大概的位置和朝向

先打开gazebo查看机器人的位置朝向，我仿真的机器人位置在坐标原点处朝向右边有轮胎的这个房间

 点击左上方的2D Pose Estimate箭头，选择坐标系原点并且长按鼠标左键选择机器人的朝向，如上图所示

 

 初始化位置完成后，过个几秒就能加载出全局代价地图和局部代价地图，上图中粉色包围的范围表示碰撞风险，离障碍物越近颜色越深，红色的实线表示全局规划的路径，点击屏幕上方的Nav2 Gaol 设置导航的目标点，机器人就可以自动规划路径并且导航到目标点了。

 我们可以通过修改navigation2的参数配置文件中的参数，来修改全局代价地图以及局部代价地图的膨胀半径，这里我将global_costmap全局代价地图和局部代价地图的膨胀半径都从原有的0.55修改为了0.3。

这是膨胀半径修改成0.3后启动导航的效果，同学也可以查阅navigation2参数配置的资料，尝试修改其他配置参数来优化导航效果。

总结

         完成以上内容，那么你已经学会了如何自主创建地图，使用navigation2在自己构建的地图中结合gazebo仿真实现机器人导航到设定好的目标点了。在此基础之上你好可以根据导航的情况自己调整参数配置文件中的参数，从而优化导航效果，你也可以在导航的路径上放置障碍物，检验导航动态避障的效果。