从头手搓一台ros2复合机器人（带机械臂）

一.前言   

        大家好呀，我们在上一小节进行了仿真，这节我们步入建图导航篇，我们需要对导航代码有一定的了解，知道是如何进行导航与定位，好了，话不多说，马上开始！

二.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实现的方案很多，但是几个比较关键的技术如下：

1. 传感器感知 通过各类传感器实现对环境的感知，比如通过激光雷达获取环境的深度信息。同时可以通过传感器融合来提高位置估计的精度，比如融合轮式里程计、IMU、雷达、深度相机数据等。

2. 视觉/激光里程计 基本原理是通过前后数据之间对比得出机器人位置的变化。

3. 回环检测 判断机器人是否到达之前到过的位置，可以解决位置估计误差问题，建图时可以纠正地图误差。

​

从算法的对数据的处理方式上看，目前常用的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 && . fishros

rosdepc 是小鱼老师制作的国内版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,就可以看到我们之间建立的地图每一个栅格的占据情况了。