ROS学习7：ROS机器人导航仿真

【Autolabor初级教程】ROS机器人入门

1. 概述

在 ROS 中机器人导航 (Navigation) 由多个功能包组合实现，ROS 中又称之为导航功能包集，关于导航模块，官方介绍如下

一个二维导航堆栈，它接收来自里程计、传感器流和目标姿态的信息，并输出发送到移动底盘的安全速度命令
ROS 中导航相关的功能包集为机器人导航提供了一套通用的实现，开发者不再需要关注于导航算法、硬件交互等偏复杂、偏底层的实现，只需专注于上层功能，而对于导航功能的调用，根据自身机器人相关参数合理设置各模块的配置文件即可，当然，也可以基于现有的功能包二次开发实现一些定制化需求，这样可以大大提高研发效率，缩短产品落地时间

1.1 导航模块简介

机器人是如何实现导航的呢？或换言之，机器人是如何从 A 点移动到 B 点呢？ROS 官方为了提供了一张导航功能包集的图示，该图中囊括了 ROS 导航的一些关键技术

假定我们已经以特定方式配置机器人，导航功能包集将使其可以运动。上图概述了这种配置方式。白色的部分是必须且已实现的组件，灰色的部分是可选且已实现的组件，蓝色的部分是必须为每一个机器人平台创建的组件。总结下来，涉及的关键技术有如下五点：

1.1.1 全局地图

在现实生活中，当我们需要实现导航时，可能会首先参考一张全局地图，然后根据地图来确定自身的位置、目的地位置，并且也会根据地图显示来规划一条大致的路线。对于机器人导航而言，也是如此，在机器人导航中地图是一个重要的组成元素，当然如果要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出：
- SLAM (simultaneous localization and mapping)，也称为 CML (Concurrent Mapping and Localization), 同时定位与地图构建，或并发建图与定位。SLAM 问题可以描述为：机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图
- 在 ROS 中，较为常用的 SLAM 实现也比较多，比如: gmapping、hector_slam、cartographer、LOAM系列、ORB_SLAM、VINS等
- 当然如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、相机等
- SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server
另外注意：SLAM 虽然是机器人导航的重要技术之一，但是二者并不等价，确切的讲，SLAM 只是实现地图构建和实时定位（SLAM不是必须的模块，因为甚至可以不要地图而是机器人自主探索前进，或者其他方式建立地图）

1.1.2 自身定位

导航伊始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，那么 GPS 是一个不错的选择，而如果室内、隧道、地下或一些特殊的屏蔽 GPS 信号的区域，由于 GPS 信号弱化甚至完全不可用，那么就必须另辟蹊径了，比如前面的 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包：amcl
- amcl (adaptive MonteCarlo Localization) 自适应的蒙特卡洛定位，是用于 2D 移动机器人的概率定位系统。它实现了自适应（或 KLD 采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态

1.1.3 路径规划

导航就是机器人从 A 点运动至 B 点的过程，在这一过程中，机器人需要根据目标位置计算全局运动路线，并且在运动过程中，还需要实时根据出现的一些动态障碍物调整运动路线，直至到达目标点，该过程就称之为路径规划。在 ROS 中提供了 move_base 包来实现路径规划，该功能包主要由两大规划器组成:
- 全局路径规划 (gloable_planner)
  - 根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线作为全局路线
- 局部路径规划 (local_planner)
  - 在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如：机器人在运行中，可能会随时出现一定的障碍物。局部路径规划的作用就是使用一定算法 (Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径
全局路径规划与局部路径规划是相对的，全局路径规划侧重于全局、宏观的实现，而局部路径规划侧重于当前、微观的实现

1.1.4 运动控制

导航功能包集假定它可以通过话题 “cmd_vel” 发布 geometry_msgs/Twist 类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅 “cmd_vel” 话题，将该话题上的速度命令转换为电机命令并发送

1.1.5 环境感知

感知周围环境信息，比如：激光雷达、相机、编码器等，激光雷达和相机可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息
在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如：SLAM、amcl、move_base 都需要依赖于环境感知

1.2 导航之坐标系

简介
- 定位是导航中的重要实现之一，所谓定位，就是参考某个坐标系 (比如：以机器人的出发点为原点创建坐标系) 在该坐标系中标注机器人。定位原理看似简单，但这个坐标系不是客观存在的，我们也无法以上帝视角确定机器人的位姿，定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种:
  - 通过里程计定位：实时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系
  - 通过传感器定位：通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系
特点
- 里程计定位
  - 优点：里程计定位信息是连续的，没有离散的跳跃
  - 缺点：里程计存在累计误差 (路面不平、测速不准、车轮打滑等)，不利于长距离或长期定位
- 传感器定位
  - 优点：比里程计定位更精准
  - 缺点：传感器定位会出现跳变的情况，且传感器定位在标志物较少的环境下，其定位精度会大打折扣
坐标系变换

上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系 (base_link 或 base_footprint)，里程计定位时，父级坐标系一般称之为 odom，如果通过传感器定位，父级参考系一般称之为 map。当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> odom -> base_link / base_footprint

1.3 导航条件说明

硬件
- 它是为差速驱动的轮式机器人设计的。它假设底盘受到理想的运动命令的控制并可实现预期的结果，命令的格式为：x 速度分量，y 速度分量，角速度 (theta) 分量
- 它需要在底盘上安装一个单线激光雷达，这个激光雷达用于构建地图和定位
- 导航功能包集是为正方形的机器人开发的，所以方形或圆形的机器人将是性能最好的。它也可以工作在任意形状和大小的机器人上，但是较大的机器人将很难通过狭窄的空间
软件
- 必须先要安装 ROS
- 当前导航基于仿真环境，先保证上一章的机器人系统仿真可以正常执行
  - 在仿真环境下，机器人可以正常接收 /cmd_vel 消息，并发布里程计消息，传感器消息发布也正常，也即导航模块中的运动控制和环境感知实现完毕

2. 导航实现

准备工作：安装相关的 ROS 功能包

$ sudo apt install ros-melodic-gmapping # 用于构建地图
$ sudo apt install ros-melodic-map-server # 用于保存与读取地图
$ sudo apt install ros-melodic-navigation # 用于定位与路径规划

新建功能包 nav_demo，并导入依赖：gmapping map_server amcl move_base

2.1 导航实现01_SLAM建图

2.2.1 gmapping 简介

gmapping 是 ROS 开源社区中较为常用且比较成熟的 SLAM 算法之一，gmapping 可以根据移动机器人里程计数据和激光雷达数据来绘制二维栅格地图，对应的，gmapping 对硬件也有一定的要求：
- 该移动机器人可以发布里程计消息
- 机器人需要发布雷达消息 (该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

2.2.2 gmapping 节点说明

gmapping 功能包中的核心节点是 slam_gmapping

订阅的 Topic
- tf (tf/tfMessage)
  - 用于雷达、底盘与里程计之间的坐标变换消息
- scan (sensor_msgs/LaserScan)
  - SLAM所需的雷达信息
发布的 Topic
- map_metadata (nav_msgs/MapMetaData)
  - 地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新
- map (nav_msgs/OccupancyGrid)
  - 地图栅格数据，一般会在 rviz 中以图形化的方式显示
- ~entropy (std_msgs/Float64)
  - 机器人姿态分布熵估计(值越大，不确定性越大)
服务
- dynamic_map (nav_msgs/GetMap)
  - 用于获取地图数据
参数(部分)
- ~base_frame (string, default：“base_link”)
  - 机器人基坐标系
- ~map_frame (string, default：“map”)
  - 地图坐标系
- ~odom_frame (string, default：“odom”)
  - 里程计坐标系
- ~map_update_interval (float, default：5.0)
  - 地图更新频率，根据指定的值设计更新间隔
- ~maxUrange (float, default：80.0)
  - 激光探测的最大可用范围 (超出此阈值，将被截断)
- ~maxRange(float)
  - 激光探测的最大范围
所需的坐标变换
- 雷达坐标系 → 基坐标系
  - 一般由 robot_state_publisher 或 static_transform_publisher 发布
- 基坐标系 → 里程计坐标系
  - 一般由里程计节点发布
发布的坐标变换
- 地图坐标系 → 里程计坐标系
  - 地图到里程计坐标系之间的变换

2.2.3 gmapping 使用

编写 gmapping 节点相关 launch 文件：nav01_slam.launch

<launch>
    <param name="use_sim_time" value="true"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <remap from="scan" to="scan"/> <!-- 雷达话题 -->
        <param name="base_frame" value="base_footprint"/> <!--底盘坐标系-->
        <param name="map_frame" value="map"/> <!--地图坐标系-->
        <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
        <param name="map_update_interval" value="5.0"/>
        <param name="maxUrange" value="16.0"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.1"/>
        <param name="srt" value="0.2"/>
        <param name="str" value="0.1"/>
        <param name="stt" value="0.2"/>
        <param name="linearUpdate" value="1.0"/>
        <param name="angularUpdate" value="0.5"/>
        <param name="temporalUpdate" value="3.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="30"/>
        <param name="xmin" value="-50.0"/>
        <param name="ymin" value="-50.0"/>
        <param name="xmax" value="50.0"/>
        <param name="ymax" value="50.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    
    <!-- 可以保存 rviz 配置并后期直接使用-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav.rviz" />
</launch>

执行地图构建
- 先启动 Gazebo 仿真环境
```
$ source ./devel/setup.bash
$ roslaunch urdf02_gazebo demo03_env.launch
```
- 然后再启动地图绘制的 launch 文件：nav01_slam.launch
```
$ source ./devel/setup.bash
$ roslaunch nav_demo nav01_slam.launch
```
- 启动键盘控制节点，用于控制机器人运动建图
```
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
```
- 在 rviz 中添加组件，显示栅格地图
最后，就可以通过键盘控制 Gazebo 中的机器人运动，同时，在 rviz 中可以显示 gmapping 发布的栅格地图数据了，下一步，还需要将地图单独保存

2.2 导航实现02_地图服务

上一节已经实现通过 gmapping 构建地图并在 rviz 中显示了地图，但地图数据是保存在内存中，当节点关闭时数据也会被一并释放

需要将栅格地图序列化到磁盘以持久化存储，后期还要通过反序列化读取磁盘的地图数据再执行后续操作。在 ROS 中，地图数据的序列化与反序列化可以通过 map_server 功能包实现

2.2.1 map_server 简介

map_server 功能包中提供了两个节点
- map_saver
  - 用于将栅格地图保存到磁盘
- map_server
  - 读取磁盘的栅格地图并以服务的方式提供出去。

2.2.2 地图保存节点 map_saver

订阅的 Topic
- map (nav_msgs/OccupancyGrid)
  - 订阅此话题用于生成地图文件
地图保存的 launch 文件：nav02_map_save.launch
```
<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>
```
SLAM 建图完毕后，执行该 launch 文件即可（需提前在功能包下新建 map 文件夹）
结果：在指定路径下 /nav_demo/map 会生成两个文件，nav.pgm 与 nav.yaml

map 保存结果解释
在这里插入图片描述

nav.pgm 本质是一张图片，直接使用图片查看程序即可打开
nav.yaml 保存的是地图的元数据信息，用于描述图片，内容格式如下

# 声明地图图片资源的路径
image: /home/用户名/demo05_ws/src/nav_demo/map/nav.pgm

# 地图刻度尺单位：米/像素
resolution: 0.050000

# 地图的位姿：按照右手坐标系（x 朝上 y 朝左 z 朝上），地图右下角相对于 rviz 中的原点位姿
    # 参数1：x 方向上的偏移量
    # 参数2：y 方向上的偏移量
    # 参数3：地图的偏航角度（单位：弧度）
origin: [-50.000000, -50.000000, 0.000000]

# 地图中的障碍物判断
    # 最终地图结果：白色是可通行区域，黑色是障碍物，灰色是未知区域
    # 判断规则
        # 1. 地图中的每个像素都有取值[0,255]：白色 255，黑色 0，像素值设为 x
        # 2. 根据像素值计算一个比例：p = (255 - x)/255 白色 0，黑色 1，灰色介于 0~1 (若 negate 为 1 ，则 p = x / 255.0)
        # 3. 判断是否是障碍物：p > occupied_thresh 就是障碍物，p < free_thresh 就是无物，可自由通行
# 占用阈值
occupied_thresh: 0.65
# 空闲阈值
free_thresh: 0.196

# 取反（换成 1 则地图中白色的地方变成黑色，黑色的变成白色）
negate: 0

2.2.3 地图服务节点 map_server

发布的话题
- map_metadata（nav_msgs / MapMetaData）
  - 发布地图元数据
- map（nav_msgs / OccupancyGrid）
  - 地图数据
服务
- static_map（nav_msgs / GetMap）
  - 通过此服务获取地图
参数
- ~frame_id（字符串，默认值：“map”）
  - 地图坐标系

地图读取：nav03_map_server.launch

运行 launch 文件

$ source ./devel/setup.bash
$ roslaunch nav_demo nav03_map_server.launch

<!-- nav03_map_server.launch -->
<launch>
    <arg name = "map" default = "nav.yaml" />

    <node name = "map_server" pkg = "map_server" type = "map_server" args = "$(find nav_demo)/map/$(arg map)"/>
</launch>

然后打开 rviz 使用 map 组件可以显示栅格地图
```
$ rviz
```

2.3 导航实现03_定位

所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM 中也包含定位算法实现，不过 SLAM 的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中
导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在 ROS 的导航功能包集 navigation 中提供了 amcl 功能包，用于实现导航中的机器人定位

2.3.1 amcl 简介

AMCL(adaptive Monte Carlo Localization) 是用于 2D 移动机器人的概率定位系统，它实现了自适应（或 KLD 采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置

2.3.2 amcl 节点说明

amcl 功能包中的核心节点是 amcl

订阅的 Topic
- scan (sensor_msgs/LaserScan)
  - 激光雷达数据
- tf (tf/tfMessage)
  - 坐标变换消息
- initialpose (geometry_msgs/PoseWithCovarianceStamped)
  - 用来初始化粒子滤波器的均值和协方差
- map (nav_msgs/OccupancyGrid)
  - 获取地图数据
发布的 Topic
- amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
  - 机器人在地图中的位姿估计
- particlecloud (geometry_msgs/PoseArray)
  - 位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合
- tf (tf/tfMessage)
  - 发布从 odom 到 map 的转换
服务
- global_localization (std_srvs/Empty)
  - 初始化全局定位的服务
- request_nomotion_update (std_srvs/Empty)
  - 手动执行更新和发布更新的粒子的服务
- set_map (nav_msgs/SetMap)
  - 手动设置新地图和姿态的服务
调用的服务
- static_map (nav_msgs/GetMap)
  - 调用此服务获取地图数据
参数（部分）
- ~odom_model_type (string, default：“diff”)
  - 里程计模型选择：“diff”,“omni”,“diff-corrected”,“omni-corrected” (diff 差速、omni 全向轮)
- ~odom_frame_id (string, default：“odom”)
  - 里程计坐标系
- ~base_frame_id (string, default：“base_link”)
  - 机器人极坐标系
- ~global_frame_id (string, default：“map”)
  - 地图坐标系
坐标变换
- 里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况如车轮打滑会出现定位错误的情况，amcl 可通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度
  - 里程计定位：只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换
  - amcl 定位：可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换

2.3.3 amcl 使用

编写 amcl 节点相关的 launch 文件：nav04_amcl.launch

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>

        <param name="odom_frame_id" value="odom"/> <!-- 里程计坐标系 -->
        <param name="base_frame_id" value="base_footprint"/> <!-- 机器人基坐标系 -->
        <param name="global_frame_id" value="map"/> <!-- 地图坐标系 -->

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

编写测试 launch 文件：test_amcl.launch

<launch>
    <!-- 启动 rviz -->
    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher"/>
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher"/>
    <node pkg = "rviz" type = "rviz" name = "rviz"/>

    <!-- 加载地图服务 -->
    <include file = "$(find nav_demo)/launch/nav03_map_server.launch" />
    
    <!-- 启动 amcl 节点 -->
    <include file = "$(find nav_demo)/launch/nav04_amcl.launch" />
</launch>

执行

先启动 Gazebo 仿真环境

$ source ./devel/setup.bash
$ roslaunch urdf02_gazebo demo03_env.launch

启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件
```
$ source ./devel/setup.bash
$ roslaunch nav_demo test_amcl.launch
```

启动键盘控制节点

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

在启动的 rviz 中，添加 RobotModel、Map 组件，分别显示机器人模型与地图，添加 posearray 插件，设置 topic 为 particlecloud 来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高
通过键盘控制机器人运动，会发现 posearray 也随之而改变

在这里插入图片描述

2.4 导航实现04_路径规划

路径规划是导航中的核心功能之一，在 ROS 的导航功能包集 navigation 中提供了 move_base 功能包，用于实现此功能

2.4.1 move_base 简介

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息
move_base 主要由全局路径规划与局部路径规划组成

2.4.2 move_base 节点说明

move_base 功能包中的核心节点是：move_base

动作订阅
- move_base/goal (move_base_msgs/MoveBaseActionGoal)
  - move_base 的运动规划目标
- move_base/cancel (actionlib_msgs/GoalID)
  - 取消目标
动作发布
- move_base/feedback (move_base_msgs/MoveBaseActionFeedback)
  - 连续反馈的信息，包含机器人底盘坐标
- move_base/status (actionlib_msgs/GoalStatusArray)
  - 发送到 move_base 的目标状态信息
- move_base/result (move_base_msgs/MoveBaseActionResult)
  - 操作结果(此处为空)
订阅的 Topic
- move_base_simple/goal (geometry_msgs/PoseStamped)
  - 运动规划目标(与 action 相比，没有连续反馈，无法追踪机器人执行状态)
发布的 Topic
- cmd_vel (geometry_msgs/Twist)
  - 输出到机器人底盘的运动控制消息
服务
- ~make_plan (nav_msgs/GetPlan)
  - 请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划
- ~clear_unknown_space (std_srvs/Empty)
  - 允许用户直接清除机器人周围的未知空间
- ~clear_costmaps (std_srvs/Empty)
  - 允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用
参数
详见move_base官网资料

2.4.3 move_base 与代价地图

概念
- 机器人导航(尤其是路径规划模块)是依赖于地图的，地图在 SLAM 时已经有所介绍了，ROS 中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过 SLAM 构建的地图在导航中是不可以直接使用的，因为：
  - SLAM 构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息
  - 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，禁止机器人进入
组成
- 代价地图有两张：global_costmap(全局代价地图) 和 local_costmap(局部代价地图)，前者用于全局路径规划，后者用于局部路径规划
- 两张代价地图都可以多层叠加，一般有以下层级:
  - Static Map Layer：静态地图层，SLAM 构建的静态地图
  - Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息
  - Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物
  - Other Layers：自定义 costmap
碰撞算法
- 致命障碍：栅格值为 254，此时障碍物与机器人中心重叠，必然发生碰撞
- 内切障碍：栅格值为 253，此时障碍物处于机器人的内切圆内，必然发生碰撞
- 外切障碍：栅格值为 [128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞
- 非自由空间：栅格值为 (0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞
- 自由区域：栅格值为 0，此处机器人可以自由通过
- 未知区域：栅格值为 255，还没探明是否有障碍物

2.4.4 move_base 使用

路径规划算法在 move_base 功能包的 move_base 节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持 ROS 的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同，最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数

具体实现

编写 launch 文件：nav05_path.launch

在功能包下新建 param 文件夹，新建文件：costmap_common_params.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml

<launch>
    <!-- respawn 为 false，意味着该节点关闭后，不会被重启 -->
    <!-- clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入 -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">

        <!-- 通过 rosparam 会载入若干 yaml 文件用于配置参数 -->
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" />
    </node>
</launch>

修改 yawl 配置文件

# costmap_common_params.yaml
# 机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 # 圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] # 其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物

inflation_radius: 0.2 # 膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
cost_scaling_factor: 3.0 # 代价比例系数，越大则代价值越小

map_type: costmap # 地图类型
observation_sources: scan # 导航包所需要的传感器

# 对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物
# 例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

# global_costmap_params.yaml
global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

# local_costmap_params.yaml
local_costmap:
  global_frame: odom #里程计坐标系
  robot_base_frame: base_footprint #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

# base_local_planner_params
TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0 # 机器人原地转圈角速度

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8 # 如果机器人行走路线飘忽不定，可将此值调大
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

参数配置技巧
以上配置在实操中，可能会出现机器人在局部路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形呢？全局路径规划与局部路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略

全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些
局部代价地图可以将膨胀半径和障碍物系数设置的偏小一些

这样，在全局路径规划时，规划的路径会尽量远离障碍物，而局部路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形

launch 文件集成：nav06_test.launch

<launch>
    <include file = "$(find nav_demo)/launch/nav03_map_server.launch" />
    <include file = "$(find nav_demo)/launch/nav04_amcl.launch" />
    <include file = "$(find nav_demo)/launch/nav05_path.launch" />

    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher"/>
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher"/>
    <node pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find nav_demo)/config/nav_test.rviz" />
</launch>

测试
- 先启动 Gazebo 仿真环境
```
$ source ./devel/setup.bash
$ roslaunch urdf02_gazebo demo03_env.launch
```
- 启动导航相关的 launch 文件
```
$ source ./devel/setup.bash
$ roslaunch nav_demo nav06_test.launch
```
- 添加 rviz 组件,可以将配置数据保存为 nav_test.rviz，后期直接调用
- 通过 rviz 工具栏的 2D Nav Goal 设置目的地实现导航
- 也可以在导航过程中，添加新的障碍物，机器人也可以自动躲避障碍物

2.4.5 导航与 SLAM 建图

背景：在导航实现 01_SLAM 建图中，是通过键盘控制机器人移动实现建图的，而后续又介绍了机器人的自主移动实现，那么可不可以将二者结合，实现机器人自主移动的 SLAM 建图呢？

上述需求是可行的。虽然可能会有疑问，导航时需要地图信息，之前导航实现时，是通过 map_server 包的 map_server 节点来发布地图信息的，如果不先通过 SLAM 建图，那么如何发布地图信息呢？
SLAM 建图过程中本身就会实时发布地图信息，所以无需再使用 map_server，SLAM 已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM 本身也是可以实现定位的

实现流程

编写 launch 文件，集成 SLAM 与 move_base 相关节点

<!-- nav07_slam_auto.launch -->
<launch>
    <include file = "$(find nav_demo)/launch/nav01_slam.launch" />
    <include file = "$(find nav_demo)/launch/nav05_path.launch" />

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav_test.rviz" />
</launch>

测试
- 先启动 Gazebo 仿真环境
```
$ source ./devel/setup.bash
$ roslaunch urdf02_gazebo demo03_env.launch
```
- 启动导航相关的 launch 文件
```
$ source ./devel/setup.bash
$ roslaunch nav_demo nav07_slam_auto.launch
```
- 在 rviz 中通过 2D Nav Goal 设置目标点，机器人开始自主移动并建图了
- 最后可以使用 map_server 保存地图
```
$ roslaunch nav_demo nav02_map_save.launch
```

3. 导航相关消息

3.1 导航之地图消息

可通过以下命令打印话题内容并输出到文件
```
$ rostopic echo /map >> map.txt
```

地图相关的消息主要有以下两个

nav_msgs/MapMetaData：地图元数据，包括地图的宽度、高度、分辨率等

time map_load_time
float32 resolution # 地图分辨率
uint32 width # 地图宽度
uint32 height # 地图高度
geometry_msgs/Pose origin # 地图位姿数据
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

nav_msgs/OccupancyGrid：地图栅格数据，一般会在 rviz 中以图形化的方式显示

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id

nav_msgs/MapMetaData info # 地图元数据
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
#--- 地图内容数据，数组长度 = width * height
int8[] data

3.2 导航之里程计消息

里程计相关消息是 nav_msgs/Odometry

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose # 里程计位姿
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist # 速度
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z    
  
  float64[36] covariance # 协方差矩阵

3.3 导航之坐标变换消息

坐标变换相关消息是 tf/tfMessage

geometry_msgs/TransformStamped[] transforms # 包含了多个坐标系相对关系数据的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

3.4 导航之定位消息

定位相关消息是 geometry_msgs/PoseArray

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose[] poses # 预估的点位姿组成的数组
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

3.5 导航之目标点与路径规划消息

目标点相关消息是 move_base_msgs/MoveBaseActionGoal

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose # 目标点位姿
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

路径规划相关消息是 nav_msgs/Path

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses # 由一系列点组成的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

3.6 导航之激光雷达消息

激光雷达相关消息是 sensor_msgs/LaserScan

显示消息内容
```
$ rosmsg info sensor_msgs/LaserScan
```

消息内容如下

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min # 起始扫描角度(rad)
float32 angle_max # 终止扫描角度(rad)
float32 angle_increment # 测量值之间的角距离(rad)
float32 time_increment # 测量间隔时间(s)
float32 scan_time # 扫描间隔时间(s)
float32 range_min # 最小有效距离值(m)
float32 range_max # 最大有效距离值(m)
float32[] ranges # 一个周期的扫描数据
float32[] intensities # 扫描强度数据，如果设备不支持强度数据，该数组为空