【ROS】给ROS小车加点Active SLAM的调料
目录
前言
一、源代码分析
1.启动文件bringup.launch分析
（1）小车通讯节点
（2）IMU相关程序分析
（3）发布静态坐标变换
（4）发布里程计坐标变换
（5）小车内的坐标变换
（6）EKF滤波程序分析
2.激光SLAM程序分析
3.PID控制器参数整定
4.建图实验
二、加点调料画彩虹
1.源代码修改
（1）地图参数修改
（2）move_base参数修改
2.仿真分析
3.建图过程
4.定位导航
总结
前言
最近我们在做ROS小车的课程设计，主要实现的功能就是定位、建图和导航，也就是使用车上提供的硬件（如：编码器、IMU、激光雷达等）运行一下小车的激光SLAM算法，实现小车的键盘控制建图、鼠标点击建图、以及在建好图上的定位和导航，最后针对具体场景具体任务提出一些针对性的创新功能。
我在队内主要负责ROS和SLAM功能搭建的工作，在运行了原有算法的前提下，对其中的代码进行了整合和改进，并优化了可视化界面，给原本平平无奇的小车加了点Sause，改进了代价地图的结构，并融入了一些Active SLAM的思想。
下面介绍一些相关的操作和原理
一、源代码分析
首先介绍一下源代码的实现逻辑，以gmapping建图算法为例，其中使用到的文件只有两个：bringup.launch和lidar_slam.launch文件。
1.启动文件bringup.launch分析
动文件bringup.launch是与下位机进行通讯，并进行各硬件初始化的总程序，它利用了launch文件的优点，使用一个命令同时启动多个节点，方便了用户的使用。
（1）小车通讯节点
首先，使用这段代码启动与STM32进行通讯的节点serial_python，并使用xml语言配置了相关参数，将端口号port设置为/dev/rikibase，将波特率设置为115200，然后使用USB和STM32进行通讯。
在使用serial库进行串口通讯之前需要建立一个serial对象，基于这个对象进行各种功能的实现。
串口初始化的过程包括设置超时、波特率、设置要打开的串口等等，其中的一些设置要与通信的另一方保持一致。
在进行初始化后，使用try-catch语句打开串口，serial::IOException& e则是serial库提供的错误提示类。
串口接收功能的实现主要依赖于这个函数：sp.read(buf,n)
我们也可以使用sp.available函数获取接收缓冲区的字节数，或者使用sp.isopen函数判断串口是否打开。
在接收数据时，我们一般建立一个接收缓存数据buf[]进行数据接收，也可以使用ROS_INFO或者std::out将数据打印输出到终端上。
发送数据主要使用sp.write(buf,n)函数，write函数有多种形式，可以在文档中查看使用，这里我使用的是第一种传入数据存储数组的首地址和发送字节长度的形式，返回值是实际写入串口的字节数，可以通过返回值判断是否发送成功，当然判断的过程在STM32端实现即可。
有关ROS串口通讯的知识可以参考我这篇博客。
（2）IMU相关程序分析
在建立了与STM32的通讯后，传递编码器和IMU的数据信息，主要用到两个节点/imu_filter_madgwick和/apply_calib节点
首先通讯节点/arduino_serial_node给/apply_calib节点发送从STM32传来的IMU数据信息，以raw_imu类型的话题发布，/apply_calib节点接收到话题后进入回调函数进行校准，如下图所示：
校准后以话题/imu/mag发送给/imu_filter_madgwick节点，在/imu_filter_madgwick节点中，将收到的校准好的IMU数据，转换成tf坐标变换进行发布，插入到tf树中，如下图所示：
另外，在/imu_filter_madgwick节点中，将一些参数设置为接口形式，方便用户在launch文件中启动时，设置相关参数，包括固定坐标系、是否发布坐标变换、世界坐标系等值。
（3）发布静态坐标变换
在进行了IMU的校准后，利用下面这行代码计算小车的初始位置，从而利用tf坐标变换包里的静态坐标变换发布器static_transform_publisher发布/base_footprint（小车底盘坐标系）和/imu_link之间的坐标变换，插入到坐标树中。
（4）发布里程计坐标变换
当小车移动时，位置不断发生变化，那么这时候我们就需要知道小车移动一段距离之后的位置，也就是初始位置和当前位置之间的坐标变换，这个坐标变换是通过里程计确定的，小车轮子转动时，通过编码器可以测得小车两个轮子的转动距离，使用riki_base_node节点转换成坐标变换关系，插入到坐标树中，即可获得小车现在位置到初始位置的坐标变换关系。
在源代码中有以下逻辑：
当该节点收到/raw_vel话题时，会进入回调函数，在回调函数中处理收到的信息，将它转化为坐标变换。
这里进行话题数据的计算和转换，在红线以下开始进行ros的tf数据的转换和存储。
这里规定了发布的坐标变换数据格式的数据头、坐标系、xyz坐标值、旋转矩阵等内容。
（5）小车内的坐标变换
一般在ROS中的移动机器人，有两个固定的坐标系，它们约定俗成为/base_footprint和/base_link，/base_footprint一般用来进行与其他坐标系之间进行坐标变换，/base_link一般在/base_footprint正上方，用来进行与小车其他部件（如摄像头）的坐标变换，/base_footprint和/base_link相对静止，这一行代码发布它们之间的坐标变换关系。
（6）EKF滤波程序分析
最后启动了一个EKF定位滤波算法节点，该节点接收tf变换的数据信息，进行卡尔曼滤波，使提供给路径规划节点的tf变换更加精确。
2.激光SLAM程序分析
本次课设用到的主要是gmapping算法，关于激光SLAM的解释和应用可以参考我这篇博客。
3.PID控制器参数整定
关于PID的介绍和原理可以参考我这篇博客，PID部分主要是封装在STM32中，但是在ROS中可以在上位机中进行可视化参数整定，可以帮助开发者摆脱反复的单片机烧写过程，代码中提供了PID参数数据信息的发布和可视化整定程序，主要由三部分构成：pid_listen、riki_pid_core、pid_configure。
pid_listen文件主要用来接收/pid话题，进入回调函数
riki_pid_core文件主要封装了几个用到的回调函数
下面这个回调函数中收到/pid这个话题后，就进入这个回调函数，在这个回调函数中报告PID的三个参数值。
pid_configure这个文件主要用来进行PID参数整定，第一个框用来进行与GUI进行交互，设置PID参数。
第二个框用来把与GUI交互设置的PID参数发布到/pid这个话题上。
与GUI进行参数调整之后，进入这个回调函数，可以将GUI设置的PID的值传入到内部参数中。
然后通过这个函数传入到msg消息中，发布出去。
4.建图实验
关于ROS环境变量配置和远程控制可以参考我这篇博客。
ssh进入小车端后，使用命令：roslaunch rikirobot bringup.launch
在虚拟机端使用命令：roslaunch rikirobot lidar_slam.launch
即可使用gmapping算法进行地图构建，因为我们之前以及构建了分布式通讯机制，因此我们可以在虚拟机端打开rviz，使用可视化界面，观察建图效果，在这里我对rviz可视化文件的配置进行了修改，提高了可视化效果，使SLAM过程更加直观鲜明，下面是实际效果：
实验环境
实验环境
运行截图
二、加点调料画彩虹
1.源代码修改
（1）地图参数修改
在ROS中地图（map）其实也是一个话题，可以由建图算法发布，由路径规划节点订阅，或者由rviz节点订阅进行可视化显示，map的数据结构如下：
std_msgs/Header header // 数据的消息头
uint32 seq // 数据的序号
time stamp // 数据的时间戳
string frame_id // 地图的坐标系
nav_msgs/MapMetaData info // 地图的一些信息
time map_load_time // 加载地图的时间
float32 resolution // 地图的分辨率，一个格子代表着多少米，一般为0.05，[m/cell]
uint32 width // 地图的宽度，像素的个数, [cells]
uint32 height // 地图的高度，像素的个数, [cells]
geometry_msgs/Pose origin // 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
// 地图数据，优先累加行，从（0,0）开始。占用值的范围为[0,100]，未知为-1。
int8[] data 比较重要的有frame_id用来描述地图的坐标系，width和height用来描述地图的高度和宽度，data这是一个变长度数组用来记录地图栅格的占用情况，ROS中地图的栅格占用情况是通过概率表示的，表示这个栅格有多大的概率被占用。
SLAM主要的依托和重要载体非地图莫属了，在gmapping算法中使用代价地图的形式进行地图存储和表示，我们优化了代价地图的参数，采用全局地图和局部地图结合的思想，不仅可以识别固定障碍物，当有动态障碍物临时出现时，也可以实现精确建图和避障。
此外，我们对障碍物的膨胀半径进行了整定，使小车在行进过程中，能够更精确地规避障碍物，以适应我们的实验环境，我们还是使用了滚动窗口优化算法，使得当有动态局部障碍物出现时，小车能更精确地规避。
（2）move_base参数修改
在定位建图过程中已经获取自身位置信息和周围障碍物信息，当小车以自身位置为起点，既定目标为终点的时候，需要对小车的行进进行路径规划，结合双层代价地图的思想，我们在ROS中通过插件的形式集成了全局和局部路径规划器，全局规划期采用global_planner/GlobalPlanner的A_star算法，局部规划器采用dwa_local_planner/DWAPlannerROS，如果该网格没用障碍物则继续往终点方向搜索，如果该网格有障碍物则停止搜索并回溯， 最终搜索到一个避障路径，不仅提高了避障巡航的稳定性和精确性，采用插件的形式极大地方便了其中的参数调整，可以使小车适应多种不同的场景。
另外，我们结合了Active SLAM的思想，采用了多层结构，使用了体素层（voxel_layer）、膨胀层（inflation_layer）和静态层（static_layer）三层代价地图辨别障碍物
同时我们采用路径队列的思想，在同一时间生成多条备选路径，通过设置不同的评价标准和权重，帮助控制器选择最优路径。
在路径规划配置文件中，我们设置了代价、距目标点距离、是否发生碰撞三个评价标准，其中是否会发生碰撞的影响因子最大，因为我们首先要保证小车的安全，我们为它的权重设为253,；距目标点的距离也很有必要，它能指导小车以更短的路径到达目的地；最后的代价影响设置为最小1，表示，我们只需要让路径规划器遵循原本的A*算法去在代价地图中规划即可。
在Rviz可视化界面中，我们通过修改配置文件，设置了队列长度，我们将候选路径设置为5，然后小车选择其中的最优路径行驶。
当用户选定一个目标点时，会生成一条全局最优路径（绿色），同时会在彩虹框（局部路径规划）中生成多条局部路径，我们的路径规划器会根据影响因子和权重选择一条最优的局部规划路径，最终到达指定点。
2.仿真分析
仿真环境：Ubuntu20.04-Gazebo
仿真模型：中科院开源urdf模型X-Bot
在这一节中，我们使用ROS提供的仿真环境-gazebo进行了实验环境的搭建，并使用X-Bot差速小车代替实物小车，完成该环境的仿真过程，仿真环境如下图所示：
我们的仿真小车为差速移动底盘，前方搭载了思岚科技A1激光雷达，上方用支撑杆托起深度相机，车体内嵌了IMU、电机、编码器等器件，基本满足实物仿真要求，小车仿真模型如下所示：
接下来我们使用gazebo和RViz进行联合仿真演示，首先演示键盘控制建图，启动键盘控制节点：rosrun robot_sim_demo robot_keyboard_teleop.py，仿真界面如下图所示：
同样在仿真环境中，我们也可以使用move_base导航框架，实现鼠标点击进行地图构建和导航，效果如下：
仿真构建地图完成：
最终构建出的地图可以使用map_server地图保存节点进行保存，最终构建的地图如下所示：
3.建图过程
我们构思的具体应用场景是送货，从我们的研究院301运送货物到研究院324，实验环境如下所示：
我们对launch文件进行了重新整合，只需要运行一行代码即可实现启动该功能，launch文件如下所示：
第二行启动了bringup.launch文件，第三行根据设置的雷达型号环境变量选择合适的雷达配置参数，第四行启动gmapping建图节点的配置参数，第十行开启了move_base，并进行相关配置。
想要启动建图功能只需要ssh进入小车端，然后执行：roslaunch rikirobot my_lidar_slam.launch
然后在虚拟机端执行：roslaunch rikirobot view_slam.launch 打开我预先配置好的RViz配置
下面是实验过程的视频截图和运行截图：
在测试过程中我们发现，如果开着摄像头进行建图，会严重影响数据传输效果，于是我们在建图过程中将摄像头关闭，在下面的定位导航过程中再将摄像头打开。
下图为小车即将到达目的地完成建图：
建图完成：
在返回过程中，由于左侧墙体并不封闭，且内部的地图没有完全构建，小车认为有路径可以通过，于是出现了下图所示情况。
我们将小车引导到有障碍物的位置，在让其直接返回初始位置，小车完全实现预计功能！
小车回到起点：
4.定位导航
在建图完毕后，我们首先使用map_server将地图保存下来以便下次直接使用：
rosrun map_server map_saver -f snyh
将这两个文件拷贝到小车的maps文件夹中，方便管理和加载。
下面我对定位导航的launch文件进行了整合：
第二行启动bringup.launch节点，第三行根据雷达型号设置的环境变量加载雷达的配置文件，第五行设置了map_server需要加载的地图文件，第六行启动map_server加载预先建好的地图，第八行启动amcl自适应蒙特卡洛定位算法，第九行启动move_base节点，加载相关的配置参数，第十行启动相机节点。
在实际运行过程中，我们采用了四机协作的方式三个从机笔记本电脑以及一个小车树莓派连接同一个手机热点：
ssh进入小车端运行：roslaunch rikirobot my_navigate.launch 打开导航算法
从机1运行rviz进行导航点设置和观察：roslaunch rikirobot view_slam.launch
从机2用来数据采集和记录，运行：rosbag record -a
从机3用来调取摄像头图像，人工保障小车安全：rqt_image_view
下面是小车启动时的截图，已经有了地图，小车不需要使用gmapping算法，因此我们打开了相机，在实验过程中图像传输使用了压缩的图像话题/img_raw，实测图像比较流畅：
小车运行过程截图：
在实验过程中，我们测试了如果小车既定路径突然出现障碍物的情况，经测试小车可以及时改变路径，重新进行规划，绕过障碍物，并顺利到达目的地，实验效果如下所示：
小车最后到达了终点：
总结
借助这次ROS小车的课程设计，我对激光SLAM的各种算法进行了复习，并学习到了更多的知识，比如上位机协助PID参数整定、深度摄像头跟随、ORB_SLAM实物操作，并将在实验室学到的move_base知识和Active SLAM知识进行了简单应用，总体来说收获很多。
老师要求3个星期结题，而我们经过夜以继日、加班加点的调试改进，使用了10天就结题了，最后还差一份精美的报告和一场精彩的答辩，纪念我大学生涯最后一次课程设计。