将数据打包为ROS包

0.引言

将图片和IMU数据打包为rosbag.我的需求是将双目图片以及IMU打包为rosbag格式.

1.Python脚本

• 参考博客.
• 参考code.

本文在上述参考上稍作修改img2bag.py：

#import cv2
import time, sys, os
from ros import rosbag
import roslib
import rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image,Imu
from geometry_msgs.msg import Vector3
# import ImageFile
from PIL import ImageFile
from PIL import Image as ImagePIL


'''sort image name'''
def CompSortFileNamesNr(f):
    g = os.path.splitext(os.path.split(f)[1])[0]
    numbertext = ''.join(c for c in g if c.isdigit())
    return int(numbertext)

'''get image from dir'''
def ReadImages(dir):
    '''Generates a list of files from the directory'''
    print( "Searching directory %s" % dir )
    all = []
    left_files = []
    right_files = []
    if os.path.exists(dir):
        for path, names, files in os.walk(dir + '/cam0/data'):
            # for f in files:
            for f in sorted(files, key=CompSortFileNamesNr):
                if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
                    if 'left' in f or 'left' in path or '/cam0/data' in f or '/cam0/data' in path :
                        left_files.append( os.path.join( path, f ) )
                    elif 'right' in f or 'right' in path or '/cam1/data' in f or '/cam1/data' in path :
                        right_files.append( os.path.join( path, f ) )
                    all.append( os.path.join( path, f ) )
        for path, names, files in os.walk(dir + '/cam1/data'):
            # for f in files:
            for f in sorted(files, key=CompSortFileNamesNr):
                if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
                    if 'left' in f or 'left' in path or '/cam0/data' in f or '/cam0/data' in path :
                        left_files.append( os.path.join( path, f ) )
                    elif 'right' in f or 'right' in path or '/cam1/data' in f or '/cam1/data' in path :
                        right_files.append( os.path.join( path, f ) )
                    all.append( os.path.join( path, f ) )
    #return all, left_files, right_files
    return left_files, right_files


def ReadIMU(filename):
    '''return IMU data and timestamp of IMU'''
    file = open(filename,'r')

    all = file.readlines()[1:]
    timestamp = []
    imu_data = []
    for f in all:
        line = f.rstrip('\n').split(',')
        timestamp.append(line[0])
        imu_data.append(line[1:])
    return timestamp,imu_data


def CreateStereoBag(args):
    '''read image'''
    left_imgs, right_imgs = ReadImages(args[0])

    '''read time stamps'''
    imagetimestamps=[]
    file = open(args[1], 'r')
    all = file.readlines()[1:] # skip the first line.
    for f in all:
        imagetimestamps.append(f.rstrip('\n').split(',')[0])
    file.close()

    '''read imu timestamps and data'''
    imutimesteps,imudata = ReadIMU(args[2])

    '''Creates a bag file containing stereo image and imu pairs'''
    #if not os.path.exists(args[3]):
    #os.system(r'touch %s' % args[3])
    bag = rosbag.Bag(args[3], 'w')


    try:
        for i in range(len(imudata)):
            imu = Imu()
            angular_v = Vector3()
            linear_a = Vector3()
            angular_v.x = float(imudata[i][0])
            angular_v.y = float(imudata[i][1])
            angular_v.z = float(imudata[i][2])
            linear_a.x = float(imudata[i][3])
            linear_a.y = float(imudata[i][4])
            linear_a.z = float(imudata[i][5])
            #imuStamp = rospy.rostime.Time.from_sec(float( (float(imutimesteps[i]))/1e6 ))
            imuStamp = rospy.rostime.Time.from_sec(float(imutimesteps[i]))
            imu.header.stamp=imuStamp
            imu.angular_velocity = angular_v
            imu.linear_acceleration = linear_a

            bag.write("/imu0",imu,imuStamp)


        for i in range(len(left_imgs)):
            print("Adding %s" % left_imgs[i])
            fp_left = open( left_imgs[i], "r" )
            p_left = ImageFile.Parser()

            '''read image size'''
            imgpil = ImagePIL.open(left_imgs[0])
            width, height = imgpil.size

            while 1:
                s = fp_left.read(1024)
                if not s:
                    break
                p_left.feed(s)

            im_left = p_left.close()

            fp_right = open( right_imgs[i], "r" )
            print("Adding %s" % right_imgs[i])
            p_right = ImageFile.Parser()

            while 1:
                s = fp_right.read(1024)
                if not s:
                    break
                p_right.feed(s)

            im_right = p_right.close()

            # Stamp = roslib.rostime.Time.from_sec(time.time())
            # Stamp = rospy.rostime.Time.from_sec(float( (float(imagetimestamps[i]))/1e6 ))
            Stamp = rospy.rostime.Time.from_sec(float(imagetimestamps[i]))

            # left image
            Img_left = Image()
            Img_left.header.stamp = Stamp
            Img_left.width = width
            Img_left.height = height
            Img_left.header.frame_id = "camera/left"

            Img_left.encoding = "mono8"
            Img_left_data = [pix for pixdata in [im_left.getdata()] for pix in pixdata]
            Img_left.step = Img_left.width
            Img_left.data = Img_left_data

            Img_right = Image()
            Img_right.header.stamp = Stamp
            Img_right.width = width
            Img_right.height = height
            Img_right.header.frame_id = "camera/right"

            Img_right.encoding = "mono8"
            Img_right_data = [pix for pixdata in [im_right.getdata()] for pix in pixdata]
            Img_right.step = Img_right.width
            Img_right.data = Img_right_data

            bag.write('/cam0/image_raw', Img_left, Stamp)
            bag.write('/cam1/image_raw', Img_right, Stamp)
    finally:
        bag.close()


if __name__ == "__main__":
    if len(sys.argv) == 5:
        print(sys.argv)
        CreateStereoBag(sys.argv[1:])
    else:
        print( "Usage: img_file,img_timestamps_file, imu_file, bagname")
        print( "Example: python3 img2bag_Stereoimu.py /*/00  data.csv imu.csv img2bag_Stereoimu.bag")
#img_file,img_timestamps_file, imu_file, bagname

使用命令：

python img2bag.py ../**  ../**/cam0/data.csv  ../**/imu0/data.csv  test.bag

只能用Python2，因为ROS链接的是python2,但是运行时会有一个问题：

struct.error: 'L' format requires 0 <= number <= 4294967295

查了一下是python2的规则限制，写入的文件大小在4G以内，但是自己的文件超过4G，目前还没找到解决办法。

2.Kalibr脚本

• 参考链接.

大神的脚本，正好前面刚刚使用过Kalibr，文件结构保持如下：

+-- dataset-dir
    +-- cam0
    │   +-- 1385030208726607500.png
    │   +--      ...
    │   \-- 1385030212176607500.png
    +-- cam1
    │   +-- 1385030208726607500.png
    │   +--      ...
    │   \-- 1385030212176607500.png
    \-- imu0.csv

切换到Kalibr工作空间，使用命令：

source devel/setup.bash
kalibr_bagcreater --folder $dataset-dir$ --output-bag $awsome.bag$

耗时比较久，建议休息的时候让它跑，后台占用还比较多，可能是自己电脑太渣.

3.其他

显然，使用ROS一边发布一边录制，肯定是能行的.可参考博客.