【从0制作自己的ros导航小车：上位机篇】04、使用gmapping建图

从0制作自己的ros导航小车

- 前言
- 一、激光雷达数据发布
- 二、激光雷达数据、小车模型、里程计数据同时显示
- 三、键盘控制小车运动
- 四、使用gmapping建图
- 五、地图保存

系列文章：
①【从0制作自己的ros导航小车：介绍及准备】
②【从0制作自己的ros导航小车：下位机篇】01、工程准备_标准库移植freertos
③【从0制作自己的ros导航小车：下位机篇】02、电机驱动、转速读取、PID控制
④【从0制作自己的ros导航小车：下位机篇】03、mpu6050偏航角获取
⑤【从0制作自己的ros导航小车：上、下位机通信篇】上、下位机串口DMA通信
⑥【从0制作自己的ros导航小车：上位机篇】01、里程计与坐标变换发布
⑦【从0制作自己的ros导航小车：上位机篇】02、ros1多机通讯与坐标变换可视化
⑧【从0制作自己的ros导航小车：上位机篇】03、添加urdf模型（发布各传感器与小车基坐标系之间的静态坐标变换）
⑨【从0制作自己的ros导航小车：上位机篇】04、使用gmapping建图
⑩【从0制作自己的ros导航小车：上位机篇】05、导航！

前言

前面的文章已经能够实现在odom坐标系下显示小车模型和里程计数据了，本文加上激光slam必不可少的传感器雷达，先进行测试，然后直接开始建图！

一、激光雷达数据发布

每个厂家的激光雷达驱动各不相同，在买来之后商家都会给出使用示例的，大家按照自己的激光雷达使用说明来测试，只需要实现/scan话题发布即可，如果官方的launch文件中有发布laser到base_footprint的坐标变换，切记注释这行代码，否则会和我们之前的urdf发布的坐标变换冲突，导致异常。
我使用的激光雷达是亚博x3单线激光雷达，按照官方的使用方法进行设置，将雷达功能包放到自己的工作空间中，然后板端运行：

roslaunch ydlidar_ros_driver X3.launch

此时在rviz中若是能看到雷达数据，那就说明没问题了
在这里插入图片描述

二、激光雷达数据、小车模型、里程计数据同时显示

为了下一步更方便的进行gmapping建图，现在先将这些预备工作测试完。在上一节的start_car.launch中继续添加代码，将雷达数据发布集成进去（静态坐标变换在urdf中以及集成，确保雷达坐标系是laser即可）。

<launch><!-- 将 urdf 文件内容设置进参数服务器 --><param name="robot_description" textfile="$(find my_description)/urdf/car.urdf" /><!-- 启动里程计与坐标变换发布节点 --><node pkg="uart_tf" type="uartandtf" name="uart_and_tf" output="screen"/><!-- 启动雷达数据发布节点 --><include file="$(find ydlidar_ros_driver)/launch/X3.launch" /><!-- 启动机器人状态和关节状态发布节点 --><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" /><node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" /></launch>

运行之后，只要在rviz中，odom坐标系下，前面所有的东西都能正常显示，那就没问题：
在这里插入图片描述

三、键盘控制小车运动

注意：stm32端的代码只需要在定时器中处理串口接收到的数据，设置电机转向和pwm即可。
运行建图时需要控制小车运动，可以使用ros中的一个键盘控制功能包：teleop_twist_keyboard。没有的话需要安装：

sudo apt-get install ros-noetic-teleop-twist-keyboard

这个节点输出的控制话题是/cmd_vel。所以我们需要在串口的cpp文件中添加一些代码以控制小车运动：

在原先代码的基础上加上下面这些，代码的位置，一看就知道加在哪里：
subscription2_ = nh_.subscribe("/cmd_vel", 20, &MyNode::cmdVelCallback, this);    ros::Subscriber subscription2_;void cmdVelCallback_daohang(const geometry_msgs::Twist::ConstPtr& msg) {double linear_vel_x = (msg->linear.x) *100;double angular_vel_th = (msg->angular.z) *100;double right_speed = 0.0;double left_speed = 0.0;right_speed = (linear_vel_x + angular_vel_th*9.5);left_speed = (linear_vel_x - angular_vel_th*9.5);writeSpeed(left_speed,right_speed,0x01);ROS_INFO("daohang:right_speed=%.2f,left_speed=%.2ff",right_speed, left_speed);
}

有了上面这个代码，就可以通过键盘控制小车运动了，记得要确保stm32端写了根据上位机下发数据进行运动的代码。上面对键盘控制节点发布的线速度和角速度进行了解算，解算过程可以参考此视频。

键盘控制节点运行：rosrun teleop_twist_keyboard teleop_twist_keyboard.py

如果此时启动键盘控制，会发现小车运动很快，并且转弯也不太行，这是因为此键盘控制节点默认的线速度是0.5，角速度是1.0，这样算出来之后可以发现数值很大，比如走直线就是50cm/s，这对于建图而言有点大了，所以在不想改代码的情况下，直接修改teleop_twist_keyboard节点的线速度角速度即可：
在这里插入图片描述
按键盘的x和c分别减小线速度与角速度，到我这样差不多就合适了。

四、使用gmapping建图

①首先安装需要的功能包（注意将ros的版本换成自己的）：

安装 gmapping 包(用于构建地图):sudo apt install ros-noetic-gmapping
安装地图服务包(用于保存与读取地图):sudo apt install ros-noetic-map-server
安装 navigation 包(用于定位以及路径规划):sudo apt install ros-noetic-navigation

②在工作空间中新建一个导航相关功能包，在功能包中新建launch文件夹，并新建launch文件

catkin_create_pkg nav gmapping map_server amcl move_base

<launch><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>
</launch>

③运行建图

1、启动前面集成好的：roslaunch uart_tf start_car.launch
2、启动gmapping：roslaunch nav gmapping.launch
3、上位机打开rviz，此时基坐标系就是map，因为运行gmapping之后，这个功能包会自动发布odom到map的坐标变换。
4、启动键盘控制节点：rosrun teleop_twist_keyboard teleop_twist_keyboard.py

在rviz中添加map、机器人模型、tf变换等参数，此时如下图所示：
在这里插入图片描述

五、地图保存

通过键盘控制小车运动，等待地图建的差不多之后调用map_server进行地图的保存，这一步是在nav功能包的launch目录下新建map_server.launch:

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

等建图完毕之后，新开终端运行此launch文件，即可将地图保存到map文件夹中：
在这里插入图片描述
建完的地图如下：