自采数据转rosbag

背景

采集数据的时候没有想到之后要是用ros的话还得转rosbag的事，所以还得后处理一下，自己也搜了一些博客，根据其他博客的内容进行了修改，分别完成了python的imu、图片和GNSS结果文件转rosbag和C++ imu、图片以及GNSS相关信息转rosbag(GNSS的还没完成)，代码的github地址是SLAM-Tool，具体在ROS文件夹

python代码实现

代码实现还是比较简单的，只要能和sensor_msg中的类型对应上就行，代码还是比较好懂的

python转IMU

# -*- coding: UTF-8 -*-
import rosbag
import sys
import os
import numpy as np
from sensor_msgs.msg import Imu
import rospy
from geometry_msgs.msg import Vector3

def readIMU(imu_path):
    timestamps = []
    wxs = []
    wys = []
    wzs = []
    axs = []
    ays = []
    azs = []
    fin = open(imu_path, 'r')
    fin.readline()
    line = fin.readline().strip()
    while line:
        parts = line.split( )
        ts = float(parts[0])+315964800+604800*2258-8*3600
        wx = float(parts[1])
        wy = float(parts[2])
        wz = float(parts[3])
        ax = float(parts[4])
        ay = float(parts[5])
        az = float(parts[6])
        timestamps.append(ts)

        wxs.append(wx)
        wys.append(wy)
        wzs.append(wz)
        axs.append(ax)
        ays.append(ay)
        azs.append(az)
        line = fin.readline().strip()
    return timestamps, wxs, wys, wzs, axs, ays, azs

if __name__ == '__main__':
    imu_path = sys.argv[1]  # IMU数据文件路径
    imu_topic_name = sys.argv[2]    # Topic名称
    bag_path = sys.argv[3]  # 输出Bag路径

    bag_out = rosbag.Bag(bag_path,'w')

    imu_ts, wxs, wys, wzs, axs, ays, azs = readIMU(imu_path)
    imu_msg = Imu()
    angular_v = Vector3()
    linear_a = Vector3()

    for i in range(len(imu_ts)):
        imu_ts_ros = rospy.rostime.Time.from_sec(imu_ts[i])
        imu_msg.header.stamp = imu_ts_ros
        
        angular_v.x = wxs[i]
        angular_v.y = wys[i]
        angular_v.z = wzs[i]

        linear_a.x = axs[i]
        linear_a.y = ays[i]
        linear_a.z = azs[i]

        imu_msg.angular_velocity = angular_v
        imu_msg.linear_acceleration = linear_a

        bag_out.write(imu_topic_name, imu_msg, imu_ts_ros)
        print('imu:',i,'/',len(imu_ts))
    
    bag_out.close()

因为文件存的是GPS周秒，所以和ROS的时间之间需要进行一个转换，ts = float(parts[0])+315964800+604800*2258-8*3600这一行就是用来干这个的，315964800是从网上找的两个时间的一个差，然后2258就是周数了
IMU这里用的是bag_out = rosbag.Bag(bag_path,‘w’)，可以简单理解为就是新建一个bag文件。因为我在采集数据的时候IMU一般都是时间最长的，所以就以IMU为基础写一个新的bag，图像和GNSS后加进去

python转GNSS

# -*- coding: UTF-8 -*-
import rosbag
import sys
import os
import numpy as np
from sensor_msgs.msg import NavSatFix
import math
import rospy
from geometry_msgs.msg import Vector3

def readGNSS(imu_path):
    timestamps = []
    lats = []
    lons = []
    alts = []
    fin = open(imu_path, 'r')
    fin.readline()
    for i in range(1,23) :
        line = fin.readline().strip()
        i=i+1
    while line:
        parts = line.split( )
        ts  = float(parts[0])+315964800+604800*2258-8*3600
        lat = float(parts[2]) * 180 / math.pi
        lon = float(parts[3]) * 180 / math.pi
        alt = float(parts[4])
        timestamps.append(ts)
        lats.append(lat)
        lons.append(lon)
        alts.append(alt)
        line = fin.readline().strip()
    return timestamps, lats, lons, alts

if __name__ == '__main__':
    gnss_path = sys.argv[1]  # GNSS数据文件路径
    gnss_topic_name = sys.argv[2]    # Topic名称
    bag_path = sys.argv[3]  # 输出Bag路径

    bag_out = rosbag.Bag(bag_path,'a')

    gnss_ts, lats, lons, alts = readGNSS(gnss_path)
    gnss_msg = NavSatFix()
    blh = Vector3()

    for i in range(len(gnss_ts)):
        gnss_ts_ros = rospy.rostime.Time.from_sec(gnss_ts[i])
        gnss_msg.header.stamp = gnss_ts_ros

        gnss_msg.latitude = lats[i]
        gnss_msg.longitude = lons[i]
        gnss_msg.altitude = alts[i]

        bag_out.write(gnss_topic_name, gnss_msg, gnss_ts_ros)
        print('gnss:',i,'/',len(gnss_ts))
    
    bag_out.close()

bag_out = rosbag.Bag(bag_path,'a')这里的a是append，就是在现有的bag上继续增加内容

image转bag

# coding=utf-8
import rosbag
import sys
import os
import numpy as np
import cv2
from cv_bridge import CvBridge
import rospy


def findFiles(root_dir, filter_type, reverse=False):
    """
    在指定目录查找指定类型文件 -> paths, names, files
    :param root_dir: 查找目录
    :param filter_type: 文件类型
    :param reverse: 是否返回倒序文件列表,默认为False
    :return: 路径、名称、文件全路径
    """

    separator = os.path.sep
    paths = []
    names = []
    files = []
    for parent, dirname, filenames in os.walk(root_dir):
        for filename in filenames:
            if filename.endswith(filter_type):
                paths.append(parent + separator)
                names.append(filename)
    for i in range(paths.__len__()):
        files.append(paths[i] + names[i])
    print(names.__len__().__str__() + " files have been found.")
    
    paths = np.array(paths)
    names = np.array(names)
    files = np.array(files)

    index = np.argsort(files)

    paths = paths[index]
    names = names[index]
    files = files[index]

    paths = list(paths)
    names = list(names)
    files = list(files)
    
    if reverse:
        paths.reverse()
        names.reverse()
        files.reverse()
    return paths, names, files

def ReadTimestamps( ):
    # 这里需修改timestamps文件的路径
    file = open("/home/zyp/DATA/106_107/107_1/image_01_dw/timestamps.txt",'r')
    all = file.readlines()
    timestamp = []
    index = 0
    for f in all:
        index = index + 1
        if index == 1:
            continue
        line = float(f.rstrip('\n'))
        timestamp.append(line+315964800+604800*2258-8*3600)
    print("Total add %i images!"%(index))
    return timestamp

if __name__ == '__main__':
    img_dir = sys.argv[1]   # 影像所在文件夹路径
    img_type = sys.argv[2]  # 影像类型
    topic_name = sys.argv[3]    # Topic名称
    bag_path = sys.argv[4]  # 输出Bag路径

    bag_out = rosbag.Bag(bag_path,'a')

    cb = CvBridge()

    paths, names, files = findFiles(img_dir,img_type)
    timestamp = ReadTimestamps()
    for i in range(len(files)):
        frame_img = cv2.imread(files[i], flags = 0)
        # timestamp = int(names[i].split(".")[0])/10e8

        ts = timestamp[i-1]
        ros_ts = rospy.rostime.Time.from_sec(ts)
        ros_img = cb.cv2_to_imgmsg(frame_img,encoding='mono8')
        ros_img.header.stamp = ros_ts
        # print(ros_img.header.stamp)
        bag_out.write(topic_name,ros_img,ros_ts)
        print('image:',i+1,'/',len(files))
    
    bag_out.close()

这里面有一个findFiles函数，也很好理解，就是用来找到文件夹下的全部文件的；以及还有一个ReadTimestamps函数，这个是用来读取时间戳的，因为图像本身没有带时间信息，所以需要再有一个单独的文件来获取时间戳。
然后image的话是先用opencv读取一下，再用cb(cv_bridge)转到imgmsg类型

C++代码实现

目前只用C++写了转imu和image的代码，之后可能会继续写转换GNSS相关数据的代码。这个代码在仓库的ROS/src/data_to_rosbag/src里

C++转IMU

/* imu数据转bag，bag包以IMU数据为基础(因为通常imu数据是时间最长的) */
void imu2bag(rosbag::Bag &bag, const std::string imuFile, const std::string outBag, int gpsWeek)
{
    std::ifstream file(imuFile);
    if (!file.is_open())
    {
        ROS_ERROR_STREAM("Failed to open file!");
        exit(1);
    }

    bag.open(outBag, rosbag::bagmode::Write);

    std::string line;
    std::getline(file, line);   // 跳过第一行

    while (std::getline(file, line))
    {
        // 将每行数据分割为各个字段
        std::istringstream iss(line);
        double time, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z;
        if (!(iss >> time >> gyro_x >> gyro_y >> gyro_z >> accel_x >> accel_y >> accel_z))
        {
            ROS_WARN_STREAM("Failed to parse line: " << line);
            continue;
        }

        // 创建IMU消息
        sensor_msgs::Imu imu_msg;
        // 315964800是GPS起始时间和计算机起始时间的一个固定差值
        time = time + 315964800 + 604800 * gpsWeek - 8 * 3600;
        imu_msg.header.stamp = ros::Time(time);
        imu_msg.angular_velocity.x = gyro_x;
        imu_msg.angular_velocity.y = gyro_y;
        imu_msg.angular_velocity.z = gyro_z;
        imu_msg.linear_acceleration.x = accel_x;
        imu_msg.linear_acceleration.y = accel_y;
        imu_msg.linear_acceleration.z = accel_z;

        // 写入ROSbag文件
        bag.write("/imu0", ros::Time(time), imu_msg);
    }

    bag.close();
    file.close();
    std::cout << "imu data convert finished!" << std::endl;
}

我是把读文件，以及写bag的操作都放在子函数里了，这样主函数只需要简单地一行调用即可

imu2bag(bag, imuFile, outBag, gpsWeek);

C++转image

/* image转bag */
void image2bag(rosbag::Bag &bag, const std::string &strPathToImage, const std::string outBag, int gpsWeek, std::string LorR)
{
    std::ifstream fTime;
    std::string strPathToTime = strPathToImage + "/timestamps.txt";
    fTime.open(strPathToTime);
    if (!fTime.is_open())
    {
        ROS_ERROR_STREAM("Failed to open timestamp file!");
        exit(1);
    }

    // 保存时间戳
    std::vector<double> vTimeStamp;
    while(!fTime.eof())
    {
        std::string s;
        getline(fTime, s);
        if (!s.empty()) {
            std::stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimeStamp.push_back(t);
        }
    }
    fTime.close();

    // 开始转换图片
    bag.open(outBag, rosbag::bagmode::Append);
    double time;
    cv::Mat image;

    for (int i =0; i < vTimeStamp.size(); i++)
    {
        std::stringstream ss;
        ss << std::setfill('0') << std::setw(10) << i;
        std::string imageName = strPathToImage + "/data/" + ss.str() + ".png"; 
        image = cv::imread(imageName, CV_LOAD_IMAGE_GRAYSCALE);
        time = vTimeStamp[i] + 315964800 + 604800 * gpsWeek - 8 * 3600;

        sensor_msgs::ImagePtr rosImg;
        cv_bridge::CvImage rosImage;
        rosImage.encoding = "mono8";
        rosImage.image = image;

        rosImg = rosImage.toImageMsg();
        rosImg->header.stamp = ros::Time(time);

        bag.write(LorR, ros::Time(time), rosImg);
        std::cout << "done: " << i << "/" << vTimeStamp.size() << std::endl; 
    }
    bag.close();
    std::cout << "image convert finished!" << std::endl;
}

这里是把时间戳读取、图片读取、图片转换都放在一个函数里了，所以外部调用也是一行

image2bag(bag, LstrPathToImage, outBag, gpsWeek, "/cam0");

因为我是双目，所以这里把topic也作为一个参数传入了，其实IMU感觉也改成这样传入topic比较好，这样就没有写死

参考致谢

博客1