ROS学习笔记08、机器人导航仿真（slam、map_server、amcl、move_base与导航消息介绍）

前言

马上开学，目前学校很多实验室都是人工智能这块，大部分都是和机器人相关，然后软件这块就是和cv、ros相关，就打算开始学习一下。

本章节是虚拟机安装Ubuntu18.04以及安装ROS的环境。

学习教程：【Autolabor初级教程】ROS机器人入门，博客中一些知识点是来源于赵老师的笔记在线笔记，本博客主要是做归纳总结，如有侵权请联系删除。

视频中的案例都基本敲了遍，这里给出我自己的源代码文件：

链接：https://pan.baidu.com/s/13CAzXk0vAWuBsc4oABC-_g 
提取码：0hws 
1
2

所有博客文件目录索引：博客目录索引(持续更新)

一、导航概述

1.1、导航模块

关键技术有如下五点:

1. 全局地图：实现导航，需要参考一张全局性质的地图，这就涉及到地图建模技术（SLAM ）。
2. 自身定位：确定自身的位置，ros中提供了定位功能包（amcl）。
3. 路径规划：从A->B点需要计算全局路线，且在运动过程中需要根据实时路况进行调整。（move_base，包含全局路径和本地实时路径）
4. 运动控制：需要借助话题"cmd_vel"发布geometry_msgs/Twist类型的消息来实现运动。
5. 环境感知：感知周围环境，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如: SLAM、amcl、move_base 都需要依赖于环境感知。

slam介绍

SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。

SLAM 实现的技术如：gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM …

• 对于地图的生成也需要进行保存：在ROS中保存地图的功能包是map_server。

完成SLAM依赖的物理传感器（需要获取周围环境深度信息的能力）：激光雷达、摄像头、RGB-D摄像头…

总结：SLAM 虽然是机器人导航的重要技术之一，但是 二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位。

amcl介绍

amcl(adaptiveMonteCarloLocalization)：自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。

move_base

该功能包由两大规划器组成：

1、全局路径规划(gloable_planner)：根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线、

2、本地时时规划(local_planner)：在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如:机器人在运行中，可能会随时出现一定的障碍物… 本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径。

全局路径规划与本地路径规划是相对的，全局路径规划侧重于全局、宏观实现，而本地路径规划侧重与当前、微观实现。

运动控制

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

环境感知

感知周围环境信息，比如: 摄像头、激光雷达、编码器…，摄像头。

例如：激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。

1.2、导航之坐标系

介绍

定位：就是参考某个坐标系(比如:以机器人的出发点为原点创建坐标系)在该坐标系中标注机器人。

定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种：

• 通过里程计定位：时时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系。
• 通过传感器定位：通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系。

两者特点

两者来进行配合最佳，前者根据行驶的距离来进行定位，后者则是依据不同的位置来进行定位：

坐标系变换

机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)

• 里程计定位：父级坐标系一般称之为 odom。
• 传感器定位：父级参考系一般称之为 map。

当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> doom -> base_link 或 base_footprint。

二、导航实现

准备工作（安装导航包和新建工程包）

安装ROS下面章节各个导航包：

# 安装 gmapping 包(用于构建地图)
sudo apt install ros-melodic-gmapping

# 安装地图服务包(用于保存与读取地图)
sudo apt install ros-melodic-map-server

# 安装 navigation 包(用于定位以及路径规划)
sudo apt install ros-melodic-navigation
1
2
3
4
5
6
7
8

创建新的工程包：

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为08nav_demo的包
catkin_create_pkg --rosdistro melodic 08nav_demo gmapping map_server amcl move_base
1
2
3
4
5

---

2.1、SLAM建图

2.1.1、认识gmapping

官网：gmapping

介绍：gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图。

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)：机器人姿态分布熵估计(值越大，不确定性越大)。

服务service

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.1.2、实操

步骤一：编写slam的launch文件

nav01_slam.launch：其中包含了gmapping的一个包对于slam的具体实现以及rviz的配置

<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="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" />

    
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
   
launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

• 其中remap表示重命名以前的话题scan为scan。
• 另外两个就是对应的坐标系名称。

步骤二：依次启动Gazebo、slam的launch文件以及键盘控制

# 启动gazebo(第7章节部分的，复用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 启动slam及rviz
roslaunch 08nav_demo nav01_slam.launch

# 启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
1
2
3
4
5
6
7
8

rviz打开之后，来添加一个map组件：指定订阅主题为/map

令小车进行自转来完成建图工作：

# 向/cmd_vel主题去发布消息
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.3"
1
2
3
4
5
6
7
8
9

---

2.2、地图服务（map_server）

2.2.1、认识map_server

通过使用gmapping的构建地图并在rviz中显示了地图，此时地图数据还是保存在内存中，此时就有个问题对于构建地图之后若是服务程序结束，那么下次就需要重新建立，如何解决呢？此时就涉及到了地图的持久化存储。

本质：会将地图数据进行序列化持久存储到磁盘中，后期会进行反序列话读取地图数据。

此时我们就可以使用ros中的map_server包来进行地图的持久化存储！

介绍map_server：map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。

订阅的topic

map(nav_msgs/OccupancyGrid)：订阅此话题用于生成地图文件。

2.2.2、实操—保存地图

前提说明：需要在SLAM建图完成之后，我们才可进行一个地图保存服务（启动2.1中的案例后再去执行该地图保存服务）。

nav02_map_save.launch：

<launch>
    <arg name="filename" value="$(find 08nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
launch>
1
2
3
4

接着我们可以去用键盘控制小车移动，等待地图绘制完成后，我们就可以去执行地图保存服务了！

# 运行地图保存服务
roslaunch 08nav_demo nav02_map_save.launch
1
2

• nav.pgm：保存的地图图片。
• nav.yaml：保存的地图配置。

效果如下：

2.2.3、实操—读取地图

nav02_map_read.launch·：

<launch>
    
    <arg name="map" default="nav.yaml" />
    
    <node name="map_server" pkg="map_server" type="map_server" args="$(find 08nav_demo)/map/$(arg map)"/>
launch>
1
2
3
4
5
6

我们可以在首先先启动地图服务读取节点，这样我们再去运行rviz后，选择map组件即可看到原先绘制的map地图了：

# 运行地图读取服务
roslaunch 08nav_demo nav02_map_read.launch
1
2

---

2.3、定位（amcl）

2.3.1、认识amcl

amcl已经被集成到了navigation包。

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

订阅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 的转换。

提供的服务service

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”)：地图坐标系。

2.3.2、坐标变换介绍

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

• 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
• amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。

---

2.3.3、实操

前提说明：需要预先准备好全局地图配置信息。

步骤一：编写amcl的配置文件

nav03_amcl.launch：

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        
        <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"/>
        
        <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_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>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

• 重点关注四个有中文注释的内容需要配置清楚。

步骤二：配置测试launch文件（集成地图读取服务、rviz以及amcl定位）

nav03_amcl_test.launch·：

<launch>
    
    <include file="$(find 08nav_demo)/launch/nav02_map_read.launch" />

    
    <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" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>

    
    <include file="$(find 08nav_demo)/launch/nav03_amcl.launch" />

launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14

接着来运行各个服务节点：

# 启动gazebo(第7章节部分的，复用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 启动amcl测试文件（集成map读取、rviz、amcl）
roslaunch 08nav_demo nav03_amcl_test.launch

# 启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
1
2
3
4
5
6
7
8

打开rviz后需要添加PoseArray组件，并且订阅/particlecloud主题：

接着我们控制键盘来让小车进行移动，接着我们来查看效果：

---

2.4、路径规划（move_base）

2.4.1、认识move_base

move_base已经被集成到了navigation包。

介绍：move_base 功能包提供了基于动作(action)的路径规划实现，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)：允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

2.4.2、move_base与代价地图

ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。

不过SLAM构建的地图在导航中是不可以直接使用的，主要原因如下：

1. SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息。
2. 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入…

静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如实时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

组成：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.3、实操—目的地导航

步骤一：编写move_base配置

nav04_path.launch：在该launch文件中包含了多个配置项

<launch>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find 08nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find 08nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find 08nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find 08nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find 08nav_demo)/param/base_local_planner_params.yaml" command="load" />
    node>

launch>
1
2
3
4
5
6
7
8
9
10
11

步骤二：编写各类配置文件

base_local_planner_params.yaml：

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

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}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

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.
1
2
3
4
5
6
7
8
9
10

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，一般与静态地图分辨率保持一致
1
2
3
4
5
6
7
8
9
10
11
12
13

步骤三：编写路径规划测试文件

nav05_path_test.launch：

<launch>
    
    <arg name="map" default="nav.yaml" />
    
    <node name="map_server" pkg="map_server" type="map_server" args="$(find 08nav_demo)/map/$(arg map)"/>
    
    

    
    <include file="$(find 08nav_demo)/launch/nav03_amcl.launch" />

    
    <include file="$(find 08nav_demo)/launch/nav04_path.launch" />

    
    <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" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

此时我们就可以来运行节点了：

# 启动gazebo(第7章节部分的，复用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 运行路径规划测试launch（地图加载、amcl、movebase、rviz）
roslaunch 08nav_demo nav05_path_test.launch
1
2
3
4
5

接着我们使用2D Nav Goal来指定目标位置来进行移动：

若是我们在行进的过程中放置障碍物，也能够实现自主避障，到达目的地：

---

2.4.4、rviz订阅全局地图、本地地图

订阅全局地图：我们可以重新在rviz中添加一个map，重命名为map_global，订阅主题为/move_base/global_costmap/costmap

订阅本地map地图：订阅主题为/move_base/global_costmap/costmap

2.5、实践—自主实现建图（slam+move_base）

nav06_auto_slam.launch：

<launch>
    
    <include file="$(find 08nav_demo)/launch/nav01_slam.launch" />

    
    <include file="$(find 08nav_demo)/launch/nav04_path.launch" />

    
    
launch>
1
2
3
4
5
6
7
8
9
10
11
12

接着运行launch文件来启动服务：

# 启动gazebo(第7章节部分的，复用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 仅仅只包含slam、move_base、rviz节点
roslaunch 08nav_demo nav06_auto_slam.launch
1
2
3
4
5

在rviz中我们打开了全局地图、本地地图，初始还没有完整建图的效果如下：

以往我们需要使用的之前的键盘来操控小车移动，这里的话我们就可以直接使用2D Naw Goal来进行手动定位目的地来完成建图！

最终的效果如下：

等待建图完成之后，我们就可以去保存全局地图了：

# 同样也是执行之前的全局地图命令保存节点
roslaunch 08nav_demo nav02_map_save.launch
1
2

三、导航消息

上面的各个导航服务集成都使用到了消息组件，对应组件就需要使用到相关消息的发布、订阅。

3.1、导航—地图

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

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

# 查看消息体命令
rosmsg info 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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

# 查看消息体命令
rosmsg info nav_msgs/OccupancyGrid

# 消息结构如下：
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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

3.2、导航—里程计

里程计相关消息是：nav_msgs/Odometry

# 查看消息体命令
rosmsg info 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

3.3、导航—坐标变换

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

# 查看消息体命令
rosmsg info 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

3.4、导航—定位

定位相关消息是：geometry_msgs/PoseArray

# 查看消息体命令
rosmsg info 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

3.4、导航—目标点与路径规划

目标点相关消息是：move_base_msgs/MoveBaseActionGoal

# 查看消息体命令
rosmsg info 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

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

# 查看消息体命令
rosmsg info 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

3.5、导航—激光雷达

激光雷达相关消息是：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 #扫描强度数据，如果设备不支持强度数据，该数组为空
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

3.6、导航—相机

对应的一般的图像数据：sensor_msgs/Image

# 查看消息体命令
rosmsg info sensor_msgs/Image

# 消息结构如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
string encoding #编码格式:RGB、YUV等
uint8 is_bigendian #图像大小端存储模式
uint32 step #一行图像数据的字节数，作为步进参数
uint8[] data #图像数据，长度等于 step * height
1
2
3
4
5
6
7
8
9
10
11
12
13
14

对应压缩后的图像数据：sensor_msgs/CompressedImage

# 查看消息体命令
rosmsg info sensor_msgs/CompressedImage

# 消息结构如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string format #压缩编码格式(jpeg、png、bmp)
uint8[] data #压缩后的数据
1
2
3
4
5
6
7
8
9
10

点云数据(带有深度信息的图像数据)：sensor_msgs/PointCloud2

# 查看消息体命令
rosmsg info sensor_msgs/PointCloud2

# 消息结构如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
sensor_msgs/PointField[] fields #每个点的数据类型
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian #图像大小端存储模式
uint32 point_step #单点的数据字节步长
uint32 row_step   #一行数据的字节步长
uint8[] data      #存储点云的数组，总长度为 row_step * height
bool is_dense     #是否有无效点
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

---

3.7、深度图像转激光数据

3.7.1、认识depthimage_to_laserscan

ros功能包：depthimage_to_laserscan，该功能包可以将深度图像信息转换成激光雷达信息。

应用场景：在诸多SLAM算法中，一般都需要订阅激光雷达数据用于构建地图，因为激光雷达可以感知周围环境的深度信息，而深度相机也具备感知深度信息的功能，且最初激光雷达价格比价比较昂贵，实际上传感器选型可以使用深度相机来去替代激光雷达。

原理：depthimage_to_laserscan将实现深度图像与雷达数据转换的原理比较简单，雷达数据是二维的、平面的，深度图像是三维的，是若干二维(水平)数据的纵向叠加，如果将三维的数据转换成二维数据，只需要取深度图的某一层即可，为了方面理解。

优缺点：

• 优点：深度相机的成本一般低于激光雷达，可以降低硬件成本；
• 缺点：深度相机较之于激光雷达无论是检测范围还是精度都有不小的差距，SLAM效果可能不如激光雷达理想。

订阅topic

image(sensor_msgs/Image)：输入图像信息。

camera_info(sensor_msgs/CameraInfo)：关联图像的相机信息。通常不需要重新映射，因为camera_info将从与image相同的命名空间中进行订阅。

发布topic

scan(sensor_msgs/LaserScan)：发布转换成的激光雷达类型数据。

参数

该节点参数较少，只有如下几个，一般需要设置的是: output_frame_id。

~scan_height(int, default: 1 pixel)：设置用于生成激光雷达信息的象素行数。

~scan_time(double, default: 1/30.0Hz (0.033s))：两次扫描的时间间隔。

~range_min(double, default: 0.45m)：返回的最小范围。结合range_max使用，只会获取 range_min 与 range_max 之间的数据。

~range_max(double, default: 10.0m)：返回的最大范围。结合range_min使用，只会获取 range_min 与 range_max 之间的数据。

~output_frame_id(str, default: camera_depth_frame)：激光信息的ID。

3.7.2、实操

# 安装depthimage-to-laserscan
sudo apt-get install ros-melodic-depthimage-to-laserscan
1
2

nav07_depthimage_to_laserscan.launch：编写launch文件执行，将深度信息转换成雷达信息

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <param name="output_frame_id" value="camera"  />
    node>
launch>
1
2
3
4
5
6

接着我们来启动各个launch文件：

# 启动gazebo(第7章节部分的，复用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 深度图像转激光数据
roslaunch 08nav_demo nav07_depthimage_to_laserscan.launch

# 运行路径规划测试launch（地图加载、amcl、movebase、rviz）
roslaunch 08nav_demo nav05_path_test.launch
1
2
3
4
5
6
7
8

在rviz中添加一个雷达扫描：

效果展示：