Apollo数据集转rosbag

本文主要介绍将Apollo开源的数据集，转化为ros可以订阅的rosbag。小白刚刚开始接触slam，没有在网上找到像转化Kitti数据集一样的工具，自己简单写了一下转化代码。

1.点云数据处理：主要是读出Apollo数据集中的PCD文件和时间戳(TXT）文件。

文件格式如下：

pcd文件：

 时间戳文件：

 

PCD文件读取代码如下（定义好PCL的输入输出头文件，以及点云格式头文件）：

//PCD 数据转化为点云(PointXYZ)
 sensor_msgs::PointCloud2 read_filelists(string dir_path,int num)
{
    pcl::PointCloud<pcl::PointXYZI> cloud;
    sensor_msgs::PointCloud2 output;
    stringstream getFiles;
    getFiles<<dir_path<<num<<".pcd";
    string path_final=getFiles.str();
    pcl::io::loadPCDFile (path_final, cloud); //修改自己pcd文件所在路径
    //Convert the cloud to ROS message
        
        pcl::toROSMsg(cloud, output);//PCD TO cloudpoint
        return output;
 }

时间戳文件读取如下（定义好数据流读取的头文件）；

while(getline(timeFile,syz))//获取每一行
        {

            stringstream ap;
            ap.clear();
            ap.str(syz);//时间戳赋值给数据流
            // while (!ap.eof())
            // {
                while (ap >> pcd_time)//读取时间戳的序号，与PCD文件序号对齐
                {
                if (num1==0)
                {
                int num2=atoi(pcd_time.c_str());//序号给numb vector
                numb.push_back(num2);
                num1++;
                continue;
                }
                if (num1==1)//时间戳信息
                {
                time=atof(pcd_time.c_str());//时间戳给 time_list vector
                times_lists.push_back(time); 
                num1=0;  /* code */
                continue;
                }
                }
        }

2.读取出imu文件：

imu数据也放在TXT文件中，格式如下，Apollo数据集的imu文件只有时间戳，加速度，角速度。

 

imu数据读取代码如下：

while (getline(fin, str))//得到每一行数据。放入定义格式的vector中
            {
            is.clear();
            is.str(str);
            while (is >> s){//这地方我的思路是循环读出每一个数据，然后每一个字符串转换成double类型的数据放入数组中，每一行结束时放入一个vec中。代码写的感觉很蠢，就只放最后一次。adad为vector（std::vector<Position> adad;）。
                if (i == 6)
                {
                   
                        double r = atof(s.c_str());    
                        temp_data[6]=r;
                        adad.push_back(Position(temp_data));
                        i=0;
                        continue;
                }

}

3.GPS数据

Apollo的GPS数据也放在TXT文件中，数据格式如下，时间戳，和车辆位姿。数据读取与IMU相同，只是多定义了一个变量。

                

4 rosbag文件读取：

首先打开

rosbag::Bag bag;
bag.open( "apollo.bag", rosbag::bagmode::Write); 

然后循环写入所需要的点云，imu，pcd数据。写入数据不区分先后，rosbag是按照时间戳发布信息。只要对应好消息的时间戳。rosbag就可以发布。

最后注意消息格式。以及对应好消息的头文件。

imu消息格式如下：

imu_data.header.stamp= ros::Time().fromSec(position[i].time);
            imu_data.header.frame_id = "imu_base_link";

            imu_data.angular_velocity.x = position[i].ax;
            imu_data.angular_velocity.y = position[i].ay;
            imu_data.angular_velocity.z = position[i].az;
    
            imu_data.linear_acceleration.x = position[i].gx;
            imu_data.linear_acceleration.y = position[i].gy;
            imu_data.linear_acceleration.z = position[i].gz;

            bag.write("imu_data", imu_data.header.stamp, imu_data);//写入rosbag

GPS消息格式如下：

            odom_data.header.stamp= ros::Time().fromSec(odomposition[i].time);
            odom_data.header.frame_id = "odom_link"；

            odom_data.pose.pose.position.x = odomposition[i].x;
            odom_data.pose.pose.position.y = odomposition[i].y;
            odom_data.pose.pose.position.z = odomposition[i].z;
    
            odom_data.pose.pose.orientation.x = odomposition[i].qx;
            odom_data.pose.pose.orientation.y = odomposition[i].qy;
            odom_data.pose.pose.orientation.z = odomposition[i].qz;
            odom_data.pose.pose.orientation.w = odomposition[i].qw;

            bag.write("odom_data", odom_data.header.stamp, odom_data);//写入rosbag

点云数据只需对应好时间戳与PCD的序号之后将返回的output写入rosbag即可。

            bag.write("velodyne_points", output.header.stamp, output);//写入rosbag