Nova-09应用参考案例1：（7）SLAM建图：cartographer

lixiang

主流的激光SLAM算法有hector、gmapping、karto、cartographer。本次使用cartographer建图。
cartographer是google开发的实时室内SLAM项目，cartographer采用基于google自家开发的ceres非线性优化的方法，cartographer采用的是主流的SLAM框架，也就是特征提取、闭环检测、后端优化的三段式。并且cartographer支持多传感器数据（odometry、IMU、LaserScan等）建图，支持2D_SLAM和3D_SLAM建图。
cartographer_ros安装
直接参考google-cartographer官方教程安装就行，官方教程分为cartographer和cartographer_ros，其实cartographer就是核心算法层、cartographer_ros是核心算法层的ros调用层。官方教程如下：
https://google-cartographer-ros.readthedocs.io/en/latest/index.html#

安装编译工具

1. sudo apt-get update
   
2. sudo apt-get install -y python-wstool python-rosdep ninja-build

复制代码

下载cartographer_ros

1. mkdir catkin_ws_carto
   
2. cd catkin_ws_carto
   
3. wstool init src
   
4. 
   
5. wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
   
6. 
   
7. wstool update -t src

复制代码

注意：在执行wstool update -t src之前，需要将src/.rosinstall文件修改成以下内容,以解决ceres-solver下载不了的问题。


安装依赖项
安装cartographer_ros的依赖项proto3、deb包等。如果执行sudo rosdep init报错，可以直接忽略。

1. src/cartographer/scripts/install_proto3.sh
   
2. sudo rosdep init
   
3. rosdep update
   
4. 
   
5. rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

复制代码

编译和安装
上面的配置和依赖都完成后，就可以开始编译和安装cartographer_ros整个项目工程了。

1. catkin_make_isolated --install --use-ninja

复制代码

注意：以后对cartographer_ros中的配置文件或源码有改动时，都需要执行这个编译命令使修改生效。

cartographer_ros使用
cartographer_ros整体代码结构
最顶层的是cartographer_ros，作为ros接口调用层，通过调用cartographer核心算法，订阅多传感器数据（/scan、/imu、/odom等），并发布地图、机器人位置信息（/map、/tf等）；其次是cartographer，作为SLAM算法的核心实现，特征提取、子图构建、闭环检测、全局优化都在这里实现，其中优化过程需要调用ceres-solver非线性优化库；最后是ceres-solver，是非线性优化库，用于求解SLAM中的优化问题。



多传感器建图配置
cartographer算法是一个非常通用和适应不同平台的开放框架算法，所以支持多种配置与工作模式。配置文件由*.lua书写被放在路径cartographer_ros/configuration_files/，我们需要建立一个我们自己的配置文件novacar_mapbuild.lua。本次采用激光雷达、IMU、轮式里程计三种传感器融合建图，所以参数一定要设置正确：
参数tracking_frame设置为imu_link，因为我们使用/imu的数据；
参数published_frame设置为odom，因为我们使用/odom的数据；
参数provide_odom_frame设置为false，因为我们使用外部/odom，所以这里不需要内部提供；
参数use_odometry设置为true，因为我们使用外部/odom的数据；
参数use_imu_data设置为true，因为我们使用/imu的数据；
参数imu_gravity_time_constant设置为10，这个是IMU的重力加速度常数。
其余参数根据需要自行调整，由于cartographer是发展很迅速的算法，所以很多代码和文档一直在更新，所以参考官方文档来解读这些参数的含义是最好的选择，官方文档连接地址如下：
https://google-cartographer-ros.readthedocs.io/en/latest/index.html

配置好参数后，再一次编译catkin_ws_carto工作空间

1. catkin_make_isolated --install --use-ninja

复制代码

启动cartographer_ros建图
启动cartographer_ros建图，分为这几个步骤：

• 启动各个传感器
• 启动cartographer_ros
• 启动rviz观察地图
• 在PC端启动键盘控制小车运动
  

启动各个传感器

1. source ~/catkin_ws/devel/setup.bash
   
2. roslaunch miiboo_bringup miiboo_all_sensor.launch

复制代码

启动cartographer_ros

1. source ~/catkin_ws_carto/install_isolated/setup.bash
   
2. roslaunch cartographer_ros miiboo_mapbuild.launch

复制代码

启动rviz观察地图
在pc端打开一个终端，运行rviz启动命令，观察地图 。

1. rosrun rviz rviz

复制代码

在rviz窗口中添加订阅/map，就可以看到地图了。

启动键盘控制小车运动
在pc端再开一个终端，运行键盘控制启动命令。

1. rosrun teleop_twist_keyboard teleop_twist_keyboard.py

复制代码

保存建图结果
扫描一圈后，地图建的也差不多了，下一步就是把建好的的地图保存下来。cartographer_ros提供了将建图结果保存为*.pbstream专门的方法。

1. source ~/catkin_ws_carto/install_isolated/setup.bash
   
2. rosservice call /write_state  /home/novauto/map/carto_map.pbstream

复制代码

地图格式转换
由于用cartographer_ros提供的/write_state方法保存的地图是*.pbstream的格式，而要在后续的自主导航中使用这个地图，我们需要将其转换为ROS中通用的GridMap格式。cartographer_ros已经给我们提供了cartographer_pbstream_to_ros_map这个节点用于转换的实现。所以，我们只需要新建一个启动文件启动这个节点就行了，文件名为novacar_pbstream2rosmap.launch，存放路径是cartographer_ros/launch/

1. <launch>
   
2.   <node name="cartographer_pbstream_to_ros_map_node" pkg="cartographer_ros"
   
3.       type="cartographer_pbstream_to_ros_map" args="
   
4.           -pbstream_filename $(arg pbstream_filename)
   
5.           -map_filestem $(arg map_filestem)"
   
6.       output="screen">
   
7.   </node>
   
8. </launch>

复制代码

配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间。

1. catkin_make_isolated --install --use-ninja

复制代码

打开终端，启动novacar_pbstream2rosmap.launch，对地图格式进行转换。

1. roslaunch cartographer_ros novacar_pbstream2rosmap.launch pbstream_filename:=/home/novauto/map/carto_map.pbstream map_filestem:=/home/novauto/map/carto_map

复制代码

运行结束后，我们可以得到转换后的地图，转换后的GridMap地图由*.pgm和*.yaml两部分构成，这时标准的ROS格式地图，可以被ROS导航框架中的map_server节点直接调用。