原链接: https://community.bwbot.org/topic/523
运行测试平台:小强ROS机器人
谷歌cartographer_ros请参考这篇安装教程：http://community.bwbot.org/topic/136/google激光雷达slam算法cartographer的安装及bag包demo测试
1. 数据包准备
使用cartographer_ros进行3d建图，需要三维点云数据和IMU数据，三维点云由速腾聚创3d激光雷达提供，IMU数据则由小强xq5开发平台提供。
rosbag数据包的详细制作过程，请参考这篇教程: https://community.bwbot.org/topic/522/使用rosbag录制和回放3d激光雷达数据和小强ros开发平台的里程计imu数据
下文教程将以蓝鲸机器人录制的bag包为例，演示如何建立三维地图。bag包百度云下载地址
2. 配置cartographer_ros
蓝鲸机器人github上的cartographer_ros已经配置好了所有文件，对于加装速腾聚创小强xq5开发平台采集的数据包，cartographer_ros需要使用的是下列文件，简略的说明在下文。cartographer_ros的详细文档可以参考官方wiki
cartographer_ros/launch/demo_xiaoqiang_3d.launch #建图时启动的launch文件，负责启动xiaoqiang_3d.launch文件和rviz
cartographer_ros/launch/xiaoqiang_3d.launch #用来加载cartographer_ros主要的启动节点和参数文件，话题数据名字的remap也在这个文件设定。
cartographer_ros/launch/assets_writer_xiaoqiang_3d.launch #用来将demo_xiaoqiang_3d.launch输出的pbstreamfile转换成ply点云数据
cartographer_ros/urdf/rslidar_2d.urd #模型文件，用来发布3d激光雷达、小车里程计、IMU、小车本体之间的tf关系
cartographer_ros/configuration_files/xiaoqiang_3d.lua #cartographer_ros算法参数配置文件，优化建图效果需要调整的参数就是这个文件。
ros中坐标系的设置，请参考这篇教程，小强xq5平台的base_link原点设置在两个主动轮轴中间位置。
# cartographer_ros)/urdf/rslidar_2d.urd文件需要调整的参数是3d雷达相对base_link的安装位置，laserbase_link等价于base_link
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="laserbase_link" />
<child link="rslidar" />
<origin xyz="0 0 0.4" rpy="0 0 0" />
</joint>
3. 启动cartographer_ros，播放rosbag数据包，开始建图
#先关闭小车的ros启动节点
#在小车主机上新开一个命令终端
sudo service startup stop
roscore
#启动cartographer
#在小车主机上新开一个命令终端
roslaunch cartographer_ros demo_xiaoqiang_3d.launch
#播放bag数据
#在小车主机上新开一个命令终端
rosbag play --clock 2018-08-11-13-20-34.bag
0_1534126256505_Screenshot from 2018-08-12 18-20-35.png
0_1534129179226_Screenshot from 2018-08-12 21-55-29.png
4. bag播放完成后，使用rosservice保存cartographer_ros建图结果，用于后续地图输出。
#打开ros service调用工具
#在小车主机上新开一个命令终端
rosrun rqt_service_caller rqt_service_caller
#先调用/finish_trajectory服务结束建图
#接着调用/write_state服务，将cartographer_ros建图结果保存为pbstream文件，test_3d.pbfile。保存成功后，文件可以在～/.ros/目录找到
0_1534127015499_Screenshot from 2018-08-12 20-42-30.png
0_1534127029035_Screenshot from 2018-08-12 20-42-57.png
关闭cartographer_ros，建图部分完成，下文继续处理pbstream文件得到点云地图文件
5.用assets_writer_xiaoqiang_3d.launch将pbstream文件转换成ply点云文件
#在小车主机上新开一个命令终端，下面是一条命令，假设bag文件和pbstream文件保存在了home目录
roslaunch cartographer_ros assets_writer_xiaoqiang_3d.launch bag_filenames:=${HOME}/2018-08-11-13-20-34.bag pose_graph_filename:=${HOME}/test_3d.pbfile
等待一段时间，处理完成后命令会自动退出，此时在bag文件旁边会生成一个.bag_points.ply后缀文件，这个就是点云文件。
0_1534127988119_2018-08-11-13-20-34.bag_xray_xy_all.png
6.使用pcl工具将ply文件转换成pcd文件
#在小车主机上新开一个命令终端，假设ply文件在home目录
pcl_ply2pcd 2018-08-11-13-20-34.bag_points.ply test_3d.pcd
转换成功后就可以得到tes_3d.pcd文件，可以直接用pcl可视化工具查看
pcl_viewer test_3d.pcd
0_1534127875497_3d_1.png
