cartographer探索：配置文件，跑自己的数据集

在实现了跑通官网数据集的基础上，再来尝试跑一下自己的数据集，许多细节地方需要注意（此时应该响起徐小虎的歌“没那么简单....”）。

目录

注意事项

实现各功能的launch文件

显示pbstream文件：已构建的地图

配置文件lua和rviz

跑自己的数据集

https://google-cartographer-ros.readthedocs.io/en/latest/your_bag.html

步骤１：配置lua文件

步骤２：编写launch文件

步骤3：编写urdf机器人模型文件

步骤4：录制自己的数据集

步骤5：开始跑数据集

关于官方数据集需要说明的几点

---

 

注意事项

1. 每次修改launch、lua等文件后也需要重新编译安装：catkin_make_isolated --install --use-ninja
2. carto使用tf2协议，不允许frame_id中包含斜杠/
3. 2d可以不需要imu，3d必须imu

 

实现各功能的launch文件

按照功能划分，分为以下几类：

（１）利用已有数据集进行2d/3d建图，如demo_backpack_2d.launch（其又调用了backpack_2d.launch）

（２）利用先验地图及数据集进行全局定位，如demo_backpack_2d_localization.launch

（３）显示pbstream文件

launch文件命名规则标明了其作用：用户根据需要选择launch文件

• offline_backpack_2d.launch：离线快速构建全局地图，事先记录的数据集被多倍快速播放
• demo_backpack_2d_localization.launch：基于先验地图进行全局定位
• demo_backpack_2d.launch：同时定位和建图，需要跑数据包
• backpack_2d.launch：同时定位和建图，使用真实的传感器数据

 

显示pbstream文件：已构建的地图

在博客https://blog.csdn.net/qq_18276949/article/details/113117562中，我们用官方数据集构建了一张2d栅格地图，该地图自动保存为.pbstream格式。

carto提供了在rviz显示pbstream文件的功能，调用以下launch即可：

roslaunch cartographer_ros visualize_pbstream.launch pbstream_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream

 

配置文件lua和rviz

https://google-cartographer-ros.readthedocs.io/en/latest/your_bag.html

https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

核心算法参数说明：

https://google-cartographer.readthedocs.io/en/latest/configuration.html#cartographer-mapping-2d-scan-matching-proto-ceresscanmatcheroptions

carto使用lua作为配置文件，lua简介：

http://www.lua.org/

https://www.runoob.com/lua/lua-tutorial.html

Lua 是一个小巧的脚本语言。作者是巴西人。该语言的设计目的是为了嵌入应用程序中，从而为应用程序提供灵活的扩展和定制功能。

Lua脚本可以很容易的被C/C++代码调用，也可以反过来调用C/C++的函数，这使得Lua在应用程序中可以被广泛应用。不仅仅作为扩展脚本，也可以作为普通的配置文件，代替XML,Ini等文件格式，并且更容易理解和维护。

Lua由标准C编写而成，代码简洁优美，几乎在所有操作系统和平台上都可以编译，运行。

一个完整的Lua解释器不过200k，在目前所有脚本引擎中，Lua的速度是最快的。这一切都决定了Lua是作为嵌入式脚本的最佳选择。

配置文件之间的调用关系：

backpack_2d_localization_evaluation.lua调用了backpack_2d_localization.lua调用了backpack_2d.lua

 

跑自己的数据集

https://google-cartographer-ros.readthedocs.io/en/latest/your_bag.html

步骤１：配置lua文件

lua、launch及urdf文件都直接拷贝原有的2d和3d的相应文件并在此基础上参照自己的传感器情况稍加修改配置即可。

步骤２：编写launch文件

 

步骤3：编写urdf机器人模型文件

carto的urdf为2个相互垂直安装的vlp16及1个imu，需要修改为我的1个vlp16加1个imu模型，在原版urdf基础上稍作修改即可。

以下展示了我添加的几个配置文件，绿色高亮的。

步骤4：录制自己的数据集

录制自己的数据集：使用vlp16和xsens G700(400hz)录制数据包，雷达安装在小四足机器人上，室外花园环境，有墙有植被，面积约50*50m。
第2排2张图为使用lio_sam建图及轨迹效果，很好。
 

 

步骤5：开始跑数据集

roslaunch cartographer_ros my_robot.launch bag_filename:=/path/to/your_bag.bag

注意！由于我自己录制的vlp16和xsens imu数据包中的imu的frame_id=/imu，这就违反了carto的tf2协议要求即不允许frame_id使用斜杠/，

因此，我自己写了一个节点来订阅我的数据集中的imu话题并修改其frame_id=imu（去掉斜杠），然后发布出来给carto订阅，这样就曲折的

解决了兼容性问题。

#ifndef IMU_TOPIC_NODE_H_
#define IMU_TOPIC_NODE_H_

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"

#include <iostream>

using std::cout;
using std::endl;

class Imu {
    public:
        Imu(ros::NodeHandle& node) : node_(node) {}
        void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg);
        void Init();
    public:
        ros::Publisher imu_pub;
        ros::Subscriber imu_sub;
    private:
        ros::NodeHandle node_;
};

void Imu::Init() {
    imu_pub = node_.advertise<sensor_msgs::Imu>("imu", 500);
    imu_sub = node_.subscribe("/imu/data", 500, &Imu::ImuHandler, this);
    cout << "Initialized." << endl;
}

void Imu::ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
    // std::cout << "imu callback." << std::endl;
    sensor_msgs::Imu imu;
    imu.header = msg->header;
    imu.header.frame_id = "imu";
    imu.orientation = msg->orientation;
    imu.orientation_covariance = msg->orientation_covariance;
    imu.angular_velocity = msg->angular_velocity;
    imu.angular_velocity_covariance = msg->angular_velocity_covariance;
    imu.linear_acceleration = msg->linear_acceleration;
    imu.linear_acceleration_covariance = msg->linear_acceleration_covariance;

    imu_pub.publish(imu);
}


int main(int argc, char** argv) {
    ros::init(argc, argv, "imu_topic");
    ros::NodeHandle node;

    Imu imu(node);
    imu.Init();
    ros::spin();

    return true;
}

#endif IMU_TOPIC_NODE_H_

 

实际跑出来的效果：2d左误差太大，3d右失败

 

 

关于官方数据集需要说明的几点

几个注意点

（１）官方自带的数据集2d和3d的都只包含以下几个话题：imu数据和激光雷达数据。

（２）可以推测出官方背包上安装了２个vlp16，分别水平和垂直放置，由于vlp16的ROS驱动可以同步输出laser_scan数据，因此不论是2d还是3d数据集其实都是用的这俩vlp16采集。

即2d和3d数据集使用的传感器（雷达和imu）完全相同，安装配置也完全相同。

（３）2d和3d数据集中的imu频率都是250HZ，但激光雷达数据的频率却不同，2d的laser_scan频率为35HZ，3d的point_cloud频率则被加速到1500HZ!（原因？论文中有讲到如果没有imu的话，那么有2种

替代方案，一是更高频率的匹配，二是像素级精度匹配算法，这里更高配的匹配速率前提就是要用高频输出的激光雷达，因此将3d点云加速到很高的频率？另外，imu的作用是针对global slam局部优化问题，imu能够提供一个较可靠且准确的前后两次匹配之间角度变化值作为局部优化问题的初始值）。补充：由于我采用的vlp16实际录制数据包的频率只有5hz，实在太低了，因此用此数据包跑出来的效果很差，跑2d时建图结果完全错误，

跑3d时基本卡在那里，然后节点挂掉，效果如上图所示。

以下显示了其中一个3d数据包话题信息：

　

 

其他注意事项

（1）回放数据包时，如何修改话题名称？改变bag文件里的消息名称。比如我有个bag文件，有个topic我想改个名字适应我的源码，可以这样做：

rosbag play file.bag /foo:=/bar    #/foo是原topic，/bar是新topic