利用Robosense 16线雷达和IMU在自己机器人上在线实时跑3D Cartographer（二）

关于3D cartographer 需要加入IMU，如果IMU 消息加入顺利，那就很快的。

下面一点一点说问题。

Launch文件

这里我试验了cartographer_node和cartographer_offline_node，都是可以成功的跑出来的。很关键的就是要把消息映射好，如果跑起来报错，打开rqt_graph，可以看到消息传递，如果是隐射对的，但是cartographer-ros没有订阅你的话题，建议查看下你的lua文件。比如我一开始

 num_point_clouds = 2

总是不订阅我的/rslidar_points而是订阅/points2_1和/points2_2。一个参数，搞了半天。

还有个参数，一开始

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160

跑的时候具体问题我记不得了，只知道和雷达硬件的覆盖角度什么有关系，但是跑不了，后面改为1，就好了。这个问题等待我测试下后在更新。

其他文件和2D差不多。下面重点讲一下IMU的消息传递，就是cartograhper 中IMU 消息发布需要注意的东西。

首先讲一下，在ROS中的四元数表示为 x y z w，那么问题来了，一定要搞明白，哪一个是实部

xi+yj+zl+w

所以实部是W，所以对应好你的IMU数据

可以安装imu-tools工具对IMU数据进行可视化。不过只能用来感觉检验四元数对不对，加速度和角速度我们发现怎么检验。

#官方文档教程
sudo apt-get install git-core
git clone https://github.com/ccny-ros-pkg/imu_tools.git
rosdep install imu_tools
rosmake imu_tools
我的rosdep过不去，索性放在catkin_ws/src下
ccatkin_make
编译，哈哈哈，编译完成，打开rviz，可以看到个imu_plugin插件

如果四元数建立的正确，可以正常显示（其中Rviz中RGB三种颜色分别对应xyz坐标轴）

如果四元数发布不对，显示错误还会自动退出报核心已转移错误。

下面就是加速度和角速度的坐标系对应问题了。也很关键。

这个我看了别人博客说的也不是太清楚，卖IMU 的博士老板给我解释了波，瞬间解决了我的疑惑。

我的IMU标的坐标系。因为没有使用说明书，所以很头疼。

在ROS中坐标系是右手坐标系，大拇指是x轴，二指为y轴，三指为Z轴，从大拇指往小拇指数，我怕写食指中指还是有人混淆。

我的imu z轴反了，x和y轴互换数据。

此外，还有imu 的协方差矩阵，imu老板让我设置个正定矩阵，但是我懒，我没处理，只给加速度、角速度和四元数赋了值。暂时没出现问题，后面如果有问题在更新。

下面就是很愉快的建图过程。

3D建图的时候，一定记得要rosbag record 

3D建图的时候，一定记得要rosbag record 

3D建图的时候，一定记得要rosbag record 

然后在保存pbstream后，可以回去处理，不放心就再跑个一遍，多录几个bag包。

处理的时候，这个命令

roslaunch cartographer_ros assets_writer_myself_3d.launch    bag_filenames:=${HOME}/2018-08-11-13-20-34.bag    pose_graph_filename:=${HOME}/test_3d.pbfile

我把我的pbstream和bag都放在文件夹/bag下

所以在这个文件夹下打开终端，roslaunch，以为这样可以省去路径，但是不行，会报错。

记得一定要是绝对路径。

之后完美的地图就出来了。