データをROSパッケージにパッケージ
データをROSパッケージにパッケージ
0.はじめに
画像とIMUデータを
rosbagにパッケージ.私のニーズは両目画像とIMUをrosbagフォーマットにパッケージすることです.1.Pythonスクリプト
本明細書では、上記の参照について、
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('
').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('
').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
Python 2しか使えません.ROSリンクはpython 2ですが、実行時に問題があります.
struct.error: 'L' format requires 0 <= number <= 4294967295
python 2のルール制限であることを調べてみると、書き込みファイルのサイズは4 G以内であるが、自分のファイルが4 Gを超えているため、まだ解決策が見つかっていない.
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を使って発表しながら録画することは、できるに違いない.ブログを参考にすることができます.