一.前言
大家好呀，我们在上一小节进行了仿真，这节我们步入建图导航篇，我们需要对导航代码有一定的了解，知道是如何进行导航与定位，好了，话不多说，马上开始！
二.ros2学习
前18小节内容可订阅往期博客
19 Slam建图
19.1 地图分类
我们根据地图所表达信息的不同可以将地图分为三类：
1. 尺度地图（Metric Map）
尺度地图用于表示尺寸距离，可以理解为把真实世界按比例缩小，尺度地图中每个点都可以使用一个经纬值进行表示。
因为尺度地图是按照真实世界按比例缩小，所以要有比例尺来表示缩小比例。
2.拓扑地图（Topological Map）
拓扑地图用于表示点与点之间的连接信息，比如地铁轨道交通图。
3.语义地图（Semantic Map）
语义地图可以理解为在上面两个地图上增加了语义，比如尺度地图中某处是红绿灯、斑马线。拓扑地图中某个点是深圳北站。
在机器人领域，尺度地图常用于定位于地图构建（Mapping）、定位（Localization）和同时定位与地图构建（Simultaneous Localization And Mapping，SLAM），拓扑地图常用于路径规划（Path Planning），而语义地图常用于人机交互（Human Robot Interaction）。
4.占据栅格地图
将地图数据分割为一块块的栅格来表达地图信息，就是栅格地图
占据（Occupancy）
机器人通过激光雷达等传感器来感知深度信息，但我们知道，传感器都是有噪声的（在前面的机器人仿真建模过程中，我们为了更加真实给激光雷达还添加了高斯噪声），所以机器人前方的某个位置到底有没有物体（障碍物）是不确定的。
我们可以采用概率来解决这一问题，认为确实有物体的栅格的占据率为100%，确定没有物体的栅格占据率为0%，不确定的栅格就用（确认占据概率/确认非占据概率）值表示占据率。
由此我们知道占据栅格地图就是一张写满占据率的格子组成的地图。
在做机器人的路径规划的时候，需要确定一个格子是否有障碍物，还是未知。所以我们一般会设定两个阈值：
- 占据阈值（occupied_thresh），比如0.65，则表示栅格占据率大于0.65的认为是有障碍物。
- 空闲阈值（free_thresh），比如0.25，则表示栅格占据率小于0.25的认为没有障碍物。
那在free_thresh和occupied_thresh之间的则认为是未知区域（未探索）。
19.2 Slam
SLAM就是解决地图和定位问题的
机器人通过自身传感器数据处理进行位置估计，同时通过不断移动完成对整个未知环境的地图构建。这就是SLAM解决的问题。
SLAM实现的方案很多，但是几个比较关键的技术如下：
-
传感器感知 通过各类传感器实现对环境的感知，比如通过激光雷达获取环境的深度信息。同时可以通过传感器融合来提高位置估计的精度，比如融合轮式里程计、IMU、雷达、深度相机数据等。
-
视觉/激光里程计 基本原理是通过前后数据之间对比得出机器人位置的变化。
-
回环检测 判断机器人是否到达之前到过的位置，可以解决位置估计误差问题，建图时可以纠正地图误差。
从算法的对数据的处理方式上看，目前常用的SLAM开源算法可以分为两类
1.基于滤波，比如扩展卡尔曼滤波（EKF: Extended Kalman Filter）、粒子滤波(PF: Particle Filter)等。
ROS中的gmapping、hector_slam算法都是基于滤波实现的。
2.基于图优化，先通过传感器进行构图，然后对图进行优化。
目前比较主流的是图优化的方法，Cartographer就是基于图优化实现的。图优化相对于滤波，不用实时的进行计算，效率更高，消耗的资源更少，所以在实际场景中使用的更多。
19.3 Cartographer介绍与安装
Cartographer是Google开源的一个可跨多个平台和传感器配置以2D和3D形式提供实时同时定位和建图（SLAM）的系统。
接下来安装Cartographer，这里采用源码安装方式
安装依赖
这里我们使用rosdepc进行依赖的安装，rosdepc指令找不到可以先运行下面的一键安装命令，选择一键配置rosdep即可。
wget http://fishros.com/install -O fishros && . fishrosrosdepc 是小鱼老师制作的国内版rosdep，是一个用于安装依赖的工具。该工具的安装可以采用一键安装进行，选项编号为3。安装完成后运行一次rodepc update即可使用。
接着在cartographer工作空间下运行下面这个命令进行依赖的安装。
编译
测试是否安装成功
先source下工作空间后再使用下面指令查看是否安装成功
cartographer_ros包含有cartographer
19.4 Cartographer参数配置
Cartographer参数是使用lua文件来描述的。
lua中的注释采用 -- 开头
前端参数
后端参数
该文件主要和地图构建
该
Carotgrapher_ROS参数配置
该部分参数主要是用于和ROS2进行通信和数据收发的配置，比如配置从哪个话题读取里程记数据，从哪个话题来获取深度信息（雷达）。
19.5 配置Fishbot进行建图
创建fishbot_cartographer
在src目录下，使用创建功能包指令，创建功能包
创建配置文件夹、launch文件夹和rviz配置文件夹
添加cartographer配置文件
在fishbot_cartographer/config目录下创建fishbot_2d.lua文件
添加launch文件
要完成使用Cartographer进行建图，需要两个节点的参与，整个过程的计算流图如下：
/cartographer_node节点:
该节点从/scan和/odom话题接收数据进行计算，输出/submap_list数据.
该节点需要接收一个参数配置文件参数。
/occupancy_grid_node节点：
该节点接收/submap_list子图列表，然后将其拼接成map并发布
该节点需要配置地图分辨率和更新周期两个参数。
编写launch文件
在路径src/fishbot_cartographer/launch/下新建cartographer.launch.py文件,将上面两个节点加入到这个launch文件中。
添加安装指令
编译启动
先启动gazebo仿真环境，因为我们的建图程序依赖于Gazebo提供雷达和里程计等数据。
启动建图
添加一下地图相关的插件
开始建图
打开机器人遥控节点，降低速度，控制机器人走一圈，看看地图的变化。
保存地图
安装nav2_map_server
保存地图
我们的地图话题为map，文件名字我们用fishbot_map,所以有下面这个这样写的命令行。
接着我们就可以得到下面的两个文件
19.6 ROS2地图加载与编辑
1.地图描述文件.yaml介绍
fishbot_map.yaml文件是地图的配置文件，该文件内容如下：
-
image：图像名称
-
mode：图像模式，默认为trinary(三进制)，还有另外两个可选项scale(缩放的)和raw(原本的值)。
-
resolution：分辨率，一个栅格对应的物理尺寸，单位为m。0.05则表示一个栅格为0.05m
-
origin：地图原点位置，单位是m。
-
negate：是否反转图像
-
cooupied_thresh：占据阈值
-
free_thresh：空闲阈值
2.地图加载
使用nav2_map_server进行地图的保存，也可以使用它来加载地图。
其中--ros-args --param yaml_filename:=fishbot_map.yaml指定地图描述文件名称.fishbot_map.yaml
map_server节点采用的是ROS2的生命周期进行编写的，除了启动节点，我们还需要手动的配置和激活节点才能使用。
打开新的终端，使用下面指令进行配置：
使用下面的指令进行激活：
将栅格数量改大,将Grid的cellsize改成0.05,就可以看到我们之间建立的地图每一个栅格的占据情况了。
