def CreateVideoBag():
right = sorted(glob.glob('pics_30-1-17/*.png'), key=numericalSort)
print "Total Images " + str(len(right))
bag = rosbag.Bag("monoBag", 'w')
cb = CvBridge()
frame_id = 565
time = 18.333333333
frame = 0.033333333
for x in range(len(right)):
time = time + frame
print time
stamp = rospy.rostime.Time.from_sec(time)
frame_id += 1
rightImg = cv2.imread(right[x])
kernel = np.zeros((9, 9), np.float32)
kernel[4, 4] = 4.0
boxFilter = np.ones((9, 9), np.float32) / 81
kernel = kernel - boxFilter
customR = cv2.filter2D(rightImg, -1, kernel)
image2 = cb.cv2_to_imgmsg(rightImg, encoding='bgr8')
image2.header.stamp = stamp
image2.header.frame_id = "camera"
#if frame_id > 175:
#print frame_id
#time = time+frame;
bag.write("camera/image_raw", image2, stamp)
#else:
# print "Missed"
bag.close()
print frame_id
def CreateStereoBag(imgs0, imgs1, odoms, hz, bagname):
'''Creates a bag file containing stereo image pairs'''
bag =rosbag.Bag(bagname, 'w')
bridge = CvBridge()
t = 100.
for i in range(len(imgs0)):
t += 1.0/float(hz)
stamp = rospy.Time.from_seconds(t)
img0 = cv2.cvtColor(imgs0[i], cv2.COLOR_BGR2GRAY)
img1 = cv2.cvtColor(imgs1[i], cv2.COLOR_BGR2GRAY)
odom = Odometry()
odom.header.stamp = stamp
odom.header.frame_id = "base_link"
odom_quat = tf.transformations.quaternion_from_euler(0, -(np.pi/2 + odoms[i][2]), 0)
odom.pose.pose = Pose(Point(-odoms[i][0]/1000., 0., odoms[i][1]/1000.), Quaternion(*odom_quat))
msg_img0 = bridge.cv2_to_imgmsg(img0, "mono8")
msg_img0.header.frame_id = "front_camera_link"
msg_img1 = bridge.cv2_to_imgmsg(img1, "mono8")
msg_img1.header.frame_id = "front_camera_link"
bag.write('front/image_raw', msg_img0, stamp)
bag.write('rear/image_raw', msg_img1, stamp)
bag.write('odom', odom, stamp)
bag.close()
def CreateVideoBag(videopath, bagname):
'''Creates a bag file with a video file'''
TOPIC = 'camera/image_raw'
bag = rosbag.Bag(bagname, 'w')
cap = cv2.VideoCapture(videopath)
cb = CvBridge()
prop_fps = cap.get(cv2.cv.CV_CAP_PROP_FPS)
if prop_fps != prop_fps or prop_fps <= 1e-2:
print "Warning: can't get FPS. Assuming 24."
prop_fps = 24
ret = True
frame_id = 0
while (ret):
ret, frame = cap.read()
if not ret:
break
stamp = rospy.rostime.Time.from_sec(float(frame_id) / prop_fps)
frame_id += 1
image = cb.cv2_to_imgmsg(frame, encoding='bgr8')
image.header.stamp = stamp
image.header.frame_id = "camera"
bag.write(TOPIC, image, stamp)
cap.release()
bag.close()
def mat2bag(self, filepath, vid_name, time_name, debug):
# Write video matrix to ROS topic in .bag file
self.vid_name = vid_name
self.time_name = time_name
self.debug = debug
self.get_file_data(filepath)
self.get_matdata()
print "Converting:", self.file_path
self.bag = rosbag.Bag(self.bag_path, 'w')
cb = CvBridge()
frame_id = 0
for frame in range(self.n_frame):
frame = self.vid[frame_id, :, :].T # get frame
if self.debug:
#print(frame_id)
cv2.imshow(self.file_name, frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
stamp = rospy.rostime.Time.from_sec(float(frame_id) / self.fs)
image = cb.cv2_to_imgmsg(frame, encoding='mono8')
image.header.stamp = stamp
frame_id += 1
image.header.frame_id = "camera"
self.bag.write(self.topic, image, stamp)
self.bag.close()
cv2.destroyAllWindows()
print "Finished: ", self.bag_path
print('')
print(
"*****************************starting to make ros bag***********************************"
)
#bagname = 'test.bag'
bagname = args.bagname
bag_indexlist = list()
bag_files = os.listdir(args.dpath)
for bag_f in bag_files:
s = (bag_f.split('_')[1])[:-4]
bag_indexlist.append(s)
reduced_bag_indexlist = bag_indexlist[0:len(bag_indexlist):4]
bridge = CvBridge()
bag = rosbag.Bag(bagname, 'w')
try:
finish = 0.0
for bagseq in reduced_bag_indexlist:
timestamp = rospy.rostime.Time.from_sec(time.time())
time.sleep(1 / 1e2)
for cam in range(0, 4, 1):
jpgname = (args.dpath + "cam{:d}_{:s}.jpg").format(cam, bagseq)
if os.path.isfile(jpgname):
imcv = read_image_cv(jpgname)
else:
imcv = np.zeros(shape=(720, 1280), dtype=np.uint8)
simg = bridge.cv2_to_imgmsg(imcv, encoding="mono8")
simg.header.frame_id = "cam{:d}".format(cam)
f = h5py.File(filepath,'r') # load file
arrays = {}
for k, v in f.items():
#print(k)
arrays[k] = np.array(v)
vid = arrays['vidData'] # video matrix
vid = np.squeeze(vid) # get rid of singleton dimension
nFrame = vid.shape[0] # # of frames
tt = arrays['t_v'] # time vector
Fs = np.round(1/np.mean(np.diff(tt)),decimals=0,out=None)/4 # frame rate
print('FRAME RATE:')
print(Fs)
bag = rosbag.Bag(bagpath, 'w')
cb = CvBridge()
frame_id = 0
for kk in range(nFrame):
frame = vid[frame_id,:,:].T # get frame
if dispFlag:
print(frame_id)
cv2.imshow('img',frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
stamp = rospy.rostime.Time.from_sec(float(frame_id) / Fs)
image = cb.cv2_to_imgmsg(frame, encoding='mono8')
image.header.stamp = stamp
frame_id +=1
def sync2bag():
parser = argparse.ArgumentParser(
description='Convert the synced lidar point cloud to rosbag')
parser.add_argument('outdir', help='output dir for outputbag')
parser.add_argument(
"dir",
nargs="?",
default=os.getcwd(),
help=
"base directory of the dataset, if no directory passed the deafult is current working directory"
)
parser.add_argument("-f",
"--first_id",
default=0,
help="first id of the sequence (between 0 - last).")
parser.add_argument("-l",
"--last_id",
default=1,
help="last id of the sequence (between 0 - last).")
args = parser.parse_args()
first_id = int(args.first_id)
last_id = int(args.last_id)
lidar_front_topic = "/lidar2/points_raw"
lidar_left_topic = "/lidar1/points_raw"
lidar_right_topic = "/lidar3/points_raw"
lidar_back_topic = "/lidar4/points_raw"
compression = rosbag.Compression.NONE
bag_filename = args.outdir + "/" + "multi_lidar_calib.bag"
lidar_front_bin_path = args.dir + "/" + "lidar_front_bin"
lidar_left_bin_path = args.dir + "/" + "lidar_left_bin"
lidar_right_bin_path = args.dir + "/" + "lidar_right_bin"
lidar_back_bin_path = args.dir + "/" + "lidar_back_bin"
lidar_front_num = len(os.listdir(lidar_front_bin_path))
if last_id > lidar_front_num:
print("last id must be smaller then " +
"{:0>2d}".format(lidar_front_num))
exit()
bag = rosbag.Bag(bag_filename, 'w', compression=compression)
frame_id = 0
frame_rate = 10.0
lidar_fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.UINT16, 1),
]
for index in range(first_id, last_id):
print("current index: " + "{:0>6d}".format(index) +
"[{:0>6d}-{:0>6d}]".format(first_id, last_id))
frame_id = frame_id + 1
lidar_front_bin_filename = lidar_front_bin_path + "/" + "{:0>6d}".format(
index) + ".bin"
lidar_left_bin_filename = lidar_left_bin_path + "/" + "{:0>6d}".format(
index) + ".bin"
lidar_right_bin_filename = lidar_right_bin_path + "/" + "{:0>6d}".format(
index) + ".bin"
lidar_back_bin_filename = lidar_back_bin_path + "/" + "{:0>6d}".format(
index) + ".bin"
stamp = rospy.rostime.Time.from_seconds(frame_id / frame_rate)
# read the lidar file bin
lidar_front_points = np.fromfile(lidar_front_bin_filename,
dtype=np.float32).reshape(-1, 4)
lidar_front_header = Header()
lidar_front_header.frame_id = "ls_front"
lidar_front_header.stamp = stamp
lidar_front_pc2 = point_cloud2.create_cloud(lidar_front_header,
lidar_fields,
lidar_front_points)
bag.write(lidar_front_topic, lidar_front_pc2, stamp)
lidar_left_points = np.fromfile(lidar_left_bin_filename,
dtype=np.float32).reshape(-1, 4)
lidar_left_header = Header()
lidar_left_header.frame_id = "ls_left"
lidar_left_header.stamp = stamp
lidar_left_pc2 = point_cloud2.create_cloud(lidar_left_header,
lidar_fields,
lidar_left_points)
bag.write(lidar_left_topic, lidar_left_pc2, stamp)
lidar_right_points = np.fromfile(lidar_right_bin_filename,
dtype=np.float32).reshape(-1, 4)
lidar_right_header = Header()
lidar_right_header.frame_id = "ls_right"
lidar_right_header.stamp = stamp
lidar_right_pc2 = point_cloud2.create_cloud(lidar_right_header,
lidar_fields,
lidar_right_points)
bag.write(lidar_right_topic, lidar_right_pc2, stamp)
lidar_back_points = np.fromfile(lidar_back_bin_filename,
dtype=np.float32).reshape(-1, 4)
lidar_back_header = Header()
lidar_back_header.frame_id = "ls_back"
lidar_back_header.stamp = stamp
lidar_back_pc2 = point_cloud2.create_cloud(lidar_back_header,
lidar_fields,
lidar_back_points)
bag.write(lidar_back_topic, lidar_back_pc2, stamp)
bag.close()
import time, sys, os
from ros import rosbag
import roslib
roslib.load_manifest('sensor_msgs')
roslib.load_manifest('rosbag')
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
import cv2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
bag_r = rosbag.Bag('test_image.bag', 'r')
bridge = CvBridge()
# topic, msg = bag_r.read_messages(topics='camera/image')
# print msg
count_total = bag_r.get_message_count()
print count_total
count = 0
# for topic, msg, t in bag_r.read_messages(topics='camera/image'):
for topic, msg, t in bag_r.read_messages(topics='camera1/image'):
if count == 0:
cv_height = msg.height
cv_width = msg.width
print cv_height
print cv_width
# cv_arr = np.zeros([msg.height,msg.width,4,count_total])
# print np.shape(cv_arr)
# print msg.format
# print count
def CreateVideoBag(videopath_R, videopath_L, bagname, yaml_file_R,
yaml_file_L):
'''Creates a bag file with a video file'''
bag = rosbag.Bag(bagname, 'w')
cap_r = cv2.VideoCapture(videopath_R)
cap_l = cv2.VideoCapture(videopath_L)
cb = CvBridge()
prop_fps = 25
ret_r = True
ret_l = True
frame_id = 0
count = 0
# Load data from file
with open(yaml_file_R, "r") as file_handle:
calib_data = yaml.load(file_handle)
camera_info_msg_R = CameraInfo()
camera_info_msg_R.width = calib_data["image_width"]
camera_info_msg_R.height = calib_data["image_height"]
camera_info_msg_R.K = calib_data["camera_matrix"]["data"]
camera_info_msg_R.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg_R.R = calib_data["rectification_matrix"]["data"]
camera_info_msg_R.P = calib_data["projection_matrix"]["data"]
camera_info_msg_R.distortion_model = calib_data["distortion_model"]
with open(yaml_file_L, "r") as file_handle:
calib_data = yaml.load(file_handle)
camera_info_msg_L = CameraInfo()
camera_info_msg_L.width = calib_data["image_width"]
camera_info_msg_L.height = calib_data["image_height"]
camera_info_msg_L.K = calib_data["camera_matrix"]["data"]
camera_info_msg_L.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg_L.R = calib_data["rectification_matrix"]["data"]
camera_info_msg_L.P = calib_data["projection_matrix"]["data"]
camera_info_msg_L.distortion_model = calib_data["distortion_model"]
#max_frame = max(cap_r.get(CV_CAP_PROP_FRAME_COUNT), cap_l.get(CV_CAP_PROP_FRAME_COUNT))
#print('Bag will record {} samples'.format(max_frame))
# for loop for right camera images
while (ret_l and ret_r):
ret_l, frame_r = cap_r.read()
ret_r, frame_l = cap_l.read()
if not (ret_l and ret_r):
break
stamp = rospy.rostime.Time.from_sec(float(frame_id) / prop_fps)
frame_id += 1
image_r = cb.cv2_to_imgmsg(frame_r, encoding='bgr8')
image_r.header.stamp = stamp
image_r.header.frame_id = "camera"
image_l = cb.cv2_to_imgmsg(frame_l, encoding='bgr8')
image_l.header.stamp = stamp
image_l.header.frame_id = "camera"
bag.write(TOPIC_R, image_r, stamp)
bag.write(TOPIC_L, image_l, stamp)
bag.write(YAML_TOPIC_R, camera_info_msg_R, stamp)
bag.write(YAML_TOPIC_L, camera_info_msg_L, stamp)
cap_r.release()
cap_l.release()
bag.close()
def extract_audio(bag_path, topic_name, mp3_path, start=None, stop=None):
global frames_per_sec
print 'Opening bag:' + bag_path
# print 'start: %d, stop: %d'%(start, stop)
bag = rosbag.Bag(bag_path + ".bag")
mp3_file = open(bag_path + "/" + bag_path + ".mp3", 'w')
print 'Reading audio messages and saving to mp3 file'
audio_msg_count = 0
video_msg_count = 0
display_message_1 = "..."
display_message_2 = "..."
display_message_3 = "..."
fingerDown = False
last_frame_time = float('nan')
frames_written = 0
secs_per_frame = 1.0 / frames_per_sec
print "secs per frame ", secs_per_frame
zero_frames_cnt = 0
audio_start_time = float('nan')
video_start_time = float('nan')
for topic, msg, stamp in bag.read_messages():
if (stop != None and stop < stamp.secs):
break
if (start != None and start > stamp.secs):
continue
# if audio topic, write to mp3 file
if msg._type == 'audio_common_msgs/AudioData':
if math.isnan(audio_start_time):
audio_start_time = to_video_time(stamp)
#if audio_msg_count < 10:
# print "audio time stamp",audio_msg_count,": ", to_video_time(stamp)
audio_msg_count += 1
mp3_file.write(''.join(msg.data))
# if image topic, write as many as are needed to maintain framerate (0-n)
if topic == image_topic:
if video_msg_count == 0:
video_start_time = to_video_time(stamp)
if math.isnan(last_frame_time):
#last_frame_time = (audio_start_time-0.1) - secs_per_frame # pretend that one frame back was written to keep logic below simpler
last_frame_time = to_video_time(stamp) - (
secs_per_frame + vid_delay_from_audio
) # pretend that one frame back was written to keep logic below simpler
print "time since audio start at first video msg: ", str(
to_video_time(stamp) - audio_start_time)
# find how many frames should be saved of this image
time_since_last_frame = to_video_time(stamp) - last_frame_time
frames_to_write = int((time_since_last_frame / secs_per_frame) +
0.5)
"""if video_msg_count < 20:
print "video time stamp",video_msg_count,": ", to_video_time(stamp)
print "time ", to_video_time(stamp)
print "writing ", frames_to_write, " frames"
print "time_since_last_frame ", time_since_last_frame, "\n" """
last_frame_time += frames_to_write * secs_per_frame
if frames_to_write == 0:
zero_frames_cnt += 1
# TODO keep track of max and min frames per sec and report to user
for i in range(0, frames_to_write):
img_filename = 'frame%010d.jpg' % (frames_written)
#if video_msg_count < 20:
# print img_filename
image_file = open(bag_path + "/images/raw/" + img_filename,
'w')
image_file.write(''.join(msg.data))
image_file.close()
if annotate:
image = Image.open(bag_path + "/images/raw/" +
img_filename)
draw = ImageDraw.Draw(image)
### font = ImageFont.truetype("arial.ttf", 20, encoding = "unic")
draw.text((10, 10),
'%d.%d' % (stamp.secs, stamp.nsecs) + ": " +
display_message_1,
fill='#ffffff') #, font=font)
draw.text((10, 20), display_message_2, fill='#ffffff')
if (fingerDown):
draw.text((10, 30), "fingerDown", fill="#ff0000")
image.save(bag_path + "/images/annotated/" + img_filename)
frames_written += 1
video_msg_count += 1
#print "image"
if topic == '/intent_to_ros':
if not 'finger' in msg.data:
try:
if 'scene' in msg.data:
j = json.loads(msg.data)
display_message_1 = j['value']
else:
j = json.loads(msg.data)
display_message_2 = j['value']
except:
display_message_2 = msg.data
if 'fingerDown' in msg.data:
fingerDown = True
if 'fingerUp' in msg.data:
fingerDown = False
bag.close()
mp3_file.close()
print 'Done.'
print '%d audio messages written to %s' % (audio_msg_count, mp3_path)
print '%d image messages seen' % (video_msg_count)
print '%d image frames written' % (frames_written)
print '%d image messages unwritten' % (zero_frames_cnt)
print '%f first video time stamp: ' % (video_start_time)
print '%f first audio time stamp: ' % (audio_start_time)
def CreateStereoBag(left_imgs, right_imgs, bagname, timestamps):
'''read time stamps'''
file = open(timestamps, 'r')
timestampslines = file.readlines()
file.close()
'''Creates a bag file containing stereo image pairs'''
bag = rosbag.Bag(bagname, 'w')
try:
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(timestampslines[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('camera/left/image_raw', Img_left, Stamp)
bag.write('camera/right/image_raw', Img_right, Stamp)
finally:
bag.close()
def CreateBag(args): #img,imu, bagname, timestamps
'''read time stamps'''
imgs = ReadImages(args[0])
imagetimestamps = []
imutimesteps, imudata = ReadIMU(args[1]) #the url of IMU data
file = open(args[3], 'r')
all = file.readlines()
for f in all:
imagetimestamps.append(f.rstrip('\n').split(' ')[1])
file.close()
'''Creates a bag file with camera images'''
if not os.path.exists(args[2]):
os.system(r'touch %s' % args[2])
bag = rosbag.Bag(args[2], '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(imutimesteps[i]))
imu.header.stamp = imuStamp
imu.angular_velocity = angular_v
imu.linear_acceleration = linear_a
bag.write("IMU/imu_raw", imu, imuStamp)
for i in range(len(imgs)):
print("Adding %s" % imgs[i])
fp = open(imgs[i], "r")
p = ImageFile.Parser()
'''read image size'''
imgpil = ImagePIL.open(imgs[0])
width, height = imgpil.size
# print "size:",width,height
while 1:
s = fp.read(1024)
if not s:
break
p.feed(s)
im = p.close()
Stamp = rospy.rostime.Time.from_sec(float(imagetimestamps[i]))
'''set image information '''
Img = Image()
Img.header.stamp = Stamp
Img.height = height
Img.width = width
Img.header.frame_id = "camera"
'''for rgb8'''
# Img.encoding = "rgb8"
# Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
# Img.step = Img.width * 3
'''for mono8'''
Img.encoding = "mono8"
Img_data = [pix for pixdata in [im.getdata()] for pix in pixdata]
Img.step = Img.width
Img.data = Img_data
bag.write('camera/image_raw', Img, Stamp)
finally:
bag.close()
def sync2bag():
parser = argparse.ArgumentParser(description='Convert the synced lidar point cloud to rosbag')
parser.add_argument('outdir', help='output dir for outputbag')
parser.add_argument("dir", nargs = "?", default = os.getcwd(), help = "base directory of the dataset, if no directory passed the deafult is current working directory")
parser.add_argument("-f", "--first_id", default=0, help = "first id of the sequence (between 0 - last).")
parser.add_argument("-l", "--last_id", default=1, help = "last id of the sequence (between 0 - last).")
args = parser.parse_args()
first_id = int(args.first_id)
last_id = int(args.last_id)
lidar_front_topic = "/lidar2/points_raw"
lidar_left_topic = "/lidar1/points_raw"
lidar_right_topic = "/lidar3/points_raw"
lidar_back_topic = "/lidar4/points_raw"
gnss_pose_topic = "/gnss_pose"
lidar_concate_topic = "/concate_points_raw"
compression = rosbag.Compression.NONE
bag_filename = args.outdir + "/" + "multi_lidar_calib.bag"
lidar_front_bin_path = args.dir + "/" + "lidar_front_bin"
lidar_left_bin_path = args.dir + "/" + "lidar_left_bin"
lidar_right_bin_path = args.dir + "/" + "lidar_right_bin"
lidar_back_bin_path = args.dir + "/" + "lidar_back_bin"
gnss_pose_path = args.dir + "/" + "vehicle_pose"
lidar_1_to_2_gt_6dof_path = args.dir
lidar_3_to_2_gt_6dof_path = args.dir
lidar_4_to_2_gt_6dof_path = args.dir
#####################################################################################################################
# load the extrinsic parameters and form the corresponding transformation matrix
lidar_1_to_2_gt_6dof_filename = lidar_1_to_2_gt_6dof_path + "/" + "lidar_1_to_2_gt_6dof.txt"
lidar_3_to_2_gt_6dof_filename = lidar_3_to_2_gt_6dof_path + "/" + "lidar_3_to_2_gt_6dof.txt"
lidar_4_to_2_gt_6dof_filename = lidar_4_to_2_gt_6dof_path + "/" + "lidar_4_to_2_gt_6dof.txt"
with open(lidar_1_to_2_gt_6dof_filename) as f:
lidar_1_to_2_gt_6dof = yaml.load(f)
with open(lidar_3_to_2_gt_6dof_filename) as f:
lidar_3_to_2_gt_6dof = yaml.load(f)
with open(lidar_4_to_2_gt_6dof_filename) as f:
lidar_4_to_2_gt_6dof = yaml.load(f)
lidar_1_to_2_T = translation_matrix((float(lidar_1_to_2_gt_6dof['x']), float(lidar_1_to_2_gt_6dof['y']), float(lidar_1_to_2_gt_6dof['z'])))
lidar_1_to_2_R = euler_matrix(float(lidar_1_to_2_gt_6dof['roll'])*math.pi/180, float(lidar_1_to_2_gt_6dof['pitch'])*math.pi/180, float(lidar_1_to_2_gt_6dof['yaw'])*math.pi/180, 'sxyz')
lidar_1_to_2_M = concatenate_matrices(lidar_1_to_2_T, lidar_1_to_2_R)
lidar_3_to_2_T = translation_matrix((float(lidar_3_to_2_gt_6dof['x']), float(lidar_3_to_2_gt_6dof['y']), float(lidar_3_to_2_gt_6dof['z'])))
lidar_3_to_2_R = euler_matrix(float(lidar_3_to_2_gt_6dof['roll'])*math.pi/180, float(lidar_3_to_2_gt_6dof['pitch'])*math.pi/180, float(lidar_3_to_2_gt_6dof['yaw'])*math.pi/180, 'sxyz')
lidar_3_to_2_M = concatenate_matrices(lidar_3_to_2_T, lidar_3_to_2_R)
lidar_4_to_2_T = translation_matrix((float(lidar_4_to_2_gt_6dof['x']), float(lidar_4_to_2_gt_6dof['y']), float(lidar_4_to_2_gt_6dof['z'])))
lidar_4_to_2_R = euler_matrix(float(lidar_4_to_2_gt_6dof['roll'])*math.pi/180, float(lidar_4_to_2_gt_6dof['pitch'])*math.pi/180, float(lidar_4_to_2_gt_6dof['yaw'])*math.pi/180, 'sxyz')
lidar_4_to_2_M = concatenate_matrices(lidar_4_to_2_T, lidar_4_to_2_R)
#####################################################################################################################
# print(lidar_1_to_2_M)
lidar_front_num = len(os.listdir(lidar_front_bin_path))
if last_id > lidar_front_num:
print("last id must be smaller then " + "{:0>2d}".format(lidar_front_num))
exit()
bag = rosbag.Bag(bag_filename, 'w', compression=compression)
frame_id = 0
frame_rate = 10.0
# lidar_fields = [ PointField('x', 0, PointField.FLOAT32, 1),
# PointField('y', 4, PointField.FLOAT32, 1),
# PointField('z', 8, PointField.FLOAT32, 1),
# PointField('intensity', 12, PointField.UINT16, 1),
# ]
lidar_fields = [ PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
]
# # just for test
# lidar_left_bin_filename = lidar_left_bin_path + "/" + "{:0>6d}".format(1) + ".bin"
# lidar_left_points = np.fromfile(lidar_left_bin_filename, dtype = np.float32).reshape(-1, 4)
# lidar_left_points_hom = lidar_left_points.copy()
# lidar_left_points_hom[..., 3] = 1
# # print(lidar_left_points)
# lidar_1_to_2_points_hom = np.dot(lidar_1_to_2_M, lidar_left_points_hom.T)
# lidar_1_to_2_points = lidar_1_to_2_points_hom.T
# lidar_1_to_2_points[..., 3] = lidar_left_points[..., 3]
# print(lidar_1_to_2_points)
# points = np.concatenate((lidar_1_to_2_points, lidar_left_points),axis=0)
# print(points)
for index in range(first_id, last_id):
print("current index: " + "{:0>6d}".format(index) + "[{:0>6d}-{:0>6d}]".format(first_id, last_id))
frame_id = frame_id + 1
lidar_front_bin_filename = lidar_front_bin_path + "/" + "{:0>6d}".format(index) + ".bin"
lidar_left_bin_filename = lidar_left_bin_path + "/" + "{:0>6d}".format(index) + ".bin"
lidar_right_bin_filename = lidar_right_bin_path + "/" + "{:0>6d}".format(index) + ".bin"
lidar_back_bin_filename = lidar_back_bin_path + "/" + "{:0>6d}".format(index) + ".bin"
gnss_pose_filename = gnss_pose_path + "/" + "{:0>6d}".format(index) + ".txt"
stamp = rospy.rostime.Time.from_seconds(frame_id/frame_rate)
with open(gnss_pose_filename) as f:
gnss_pose_6dof = yaml.load(f)
gnss_pose_header = Header()
gnss_pose_header.frame_id = "gnss"
gnss_pose_header.stamp = stamp
gnss_poseStamp = PoseStamped()
gnss_poseStamp.header = gnss_pose_header
gnss_poseStamp.pose.position.x = gnss_pose_6dof['x']
gnss_poseStamp.pose.position.y = gnss_pose_6dof['y']
gnss_poseStamp.pose.position.z = gnss_pose_6dof['z']
quat = quaternion_from_euler(float(gnss_pose_6dof['roll'])*math.pi/180, float(gnss_pose_6dof['pitch'])*math.pi/180, float(gnss_pose_6dof['yaw'])*math.pi/180, axes='sxyz')
gnss_poseStamp.pose.orientation.x = quat[0]
gnss_poseStamp.pose.orientation.y = quat[1]
gnss_poseStamp.pose.orientation.z = quat[2]
gnss_poseStamp.pose.orientation.w = quat[3]
bag.write(gnss_pose_topic, gnss_poseStamp, stamp)
# read the lidar file bin
lidar_front_points = np.fromfile(lidar_front_bin_filename, dtype = np.float32).reshape(-1, 4)
# print(lidar_front_points)
lidar_front_header = Header()
lidar_front_header.frame_id = "ls_front"
lidar_front_header.stamp = stamp
lidar_front_pc2 = point_cloud2.create_cloud(lidar_front_header, lidar_fields, lidar_front_points)
bag.write(lidar_front_topic, lidar_front_pc2, stamp)
lidar_left_points = np.fromfile(lidar_left_bin_filename, dtype = np.float32).reshape(-1, 4)
lidar_left_header = Header()
lidar_left_header.frame_id = "ls_left"
lidar_left_header.stamp = stamp
lidar_left_pc2 = point_cloud2.create_cloud(lidar_left_header, lidar_fields, lidar_left_points)
bag.write(lidar_left_topic, lidar_left_pc2, stamp)
lidar_right_points = np.fromfile(lidar_right_bin_filename, dtype = np.float32).reshape(-1, 4)
lidar_right_header = Header()
lidar_right_header.frame_id = "ls_right"
lidar_right_header.stamp = stamp
lidar_right_pc2 = point_cloud2.create_cloud(lidar_right_header, lidar_fields, lidar_right_points)
bag.write(lidar_right_topic, lidar_right_pc2, stamp)
lidar_back_points = np.fromfile(lidar_back_bin_filename, dtype = np.float32).reshape(-1, 4)
lidar_back_header = Header()
lidar_back_header.frame_id = "ls_back"
lidar_back_header.stamp = stamp
lidar_back_pc2 = point_cloud2.create_cloud(lidar_back_header, lidar_fields, lidar_back_points)
bag.write(lidar_back_topic, lidar_back_pc2, stamp)
# concate the point cloud from front, left, right and back
# transform left points to front coordinates
lidar_left_points_hom = lidar_left_points.copy()
lidar_left_points_hom[..., 3] = 1
lidar_1_to_2_points_hom = np.dot(lidar_1_to_2_M, lidar_left_points_hom.T)
lidar_1_to_2_points = lidar_1_to_2_points_hom.T
lidar_1_to_2_points[..., 3] = lidar_left_points[..., 3]
# transform right points to front coordinates
lidar_right_points_hom = lidar_right_points.copy()
lidar_right_points_hom[..., 3] = 1
lidar_3_to_2_points_hom = np.dot(lidar_3_to_2_M, lidar_right_points_hom.T)
lidar_3_to_2_points = lidar_3_to_2_points_hom.T
lidar_3_to_2_points[..., 3] = lidar_right_points[..., 3]
# transform back points to front coordinates
lidar_back_points_hom = lidar_back_points.copy()
lidar_back_points_hom[..., 3] = 1
lidar_4_to_2_points_hom = np.dot(lidar_4_to_2_M, lidar_back_points_hom.T)
lidar_4_to_2_points = lidar_4_to_2_points_hom.T
lidar_4_to_2_points[..., 3] = lidar_back_points[..., 3]
lidar_concate_points = np.concatenate((lidar_front_points, lidar_1_to_2_points),axis=0)
lidar_concate_points = np.concatenate((lidar_concate_points, lidar_3_to_2_points),axis=0)
lidar_concate_points = np.concatenate((lidar_concate_points, lidar_4_to_2_points),axis=0)
lidar_concate_header = Header()
lidar_concate_header.frame_id = "ls_concate"
lidar_concate_header.stamp = stamp
lidar_concate_pc2 = point_cloud2.create_cloud(lidar_concate_header, lidar_fields, lidar_concate_points)
bag.write(lidar_concate_topic, lidar_concate_pc2, stamp)
bag.close()
else:
tst = [int(sys.argv[2])]
print('Creating rosbags for sequences in '+ str(tst))
# Delete first frames
start=0.5
# Loop trough all sequences.
for i in iter(tst):
# Find folder
dir =folder+'/advio-'+str(i).zfill(2)+'/iphone'
print(dir)
# Initialize bag, data and video reader.
bag =rosbag.Bag(dir+'/'+'iphone.bag', 'w')
index=0
csvfile=open(dir+'/'+'imu-gyro.csv')
datareader = csv.reader(csvfile, delimiter=' ', quotechar='|')
# Initialize bridge.
bridge=CvBridge()
# Read video and check for number of frames.
vidcap = cv2.VideoCapture(dir+'/'+'frames.mov')
vlength = int(vidcap.get(cv2.CAP_PROP_FRAME_COUNT))
# For each row in the data matrix
for row in datareader:
# Read data row
sp=row[0].split(",")
def CreateBag(args): #img,imu, bagname, timestamps
'''read time stamps'''
imgs = ReadImages(args[0])
imagetimestamps = []
imutimesteps, imudata = ReadIMU(args[1]) #the url of IMU data
file = open(args[3], 'r')
all = file.readlines()
for f in all:
imagetimestamps.append(f.rstrip('\n'))
# print imagetimestamps
# print imutimesteps
file.close()
'''Creates a bag file with camera images'''
if not os.path.exists(args[2]):
os.system(r'touch %s' % args[2])
bag = rosbag.Bag(args[2], 'w')
try:
for i in range(len(imudata)):
imu = Imu()
angular_v = Vector3()
linear_a = Vector3()
angular_v.x = float(imudata[i][3])
angular_v.y = float(imudata[i][4])
angular_v.z = float(imudata[i][5])
linear_a.x = float(imudata[i][0])
linear_a.y = float(imudata[i][1])
linear_a.z = float(imudata[i][2])
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("IMU/imu_raw", imu, imuStamp)
for i in range(len(imgs)):
print("Adding %s" % imgs[i])
fp = open(imgs[i], "r")
Stamp = rospy.rostime.Time.from_sec(float(imagetimestamps[i]))
frame = cv2.imread(imgs[i], 0)
cb = CvBridge()
img_ros = cb.cv2_to_imgmsg(frame, encoding='mono8')
img_ros.header.stamp = Stamp
img_ros.header.frame_id = "camera"
bag.write('camera/image_raw', img_ros, Stamp)
# cv2.imshow("img",img)
# cv2.waitKey(0)
## python PIL image lib
# p = ImageFile.Parser()
# '''read image size'''
# imgpil = ImagePIL.open(imgs[0])
#
# width, height = imgpil.size
# print "size:", width, height
#
# while 1:
# s = fp.read(1024)
# if not s:
# break
# p.feed(s)
#
# im = p.close()
#
# '''set image information '''
# Img = Image()
#
# Img.header.stamp = Stamp
# Img.height = height
# Img.width = width
# Img.header.frame_id = "camera"
#
# '''for rgb8'''
# # Img.encoding = "rgb8"
# # Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
# # Img.step = Img.width * 3
#
# '''for mono8'''
# Img.encoding = "mono8"
# Img_data = [pix for pixdata in [im.getdata()] for pix in pixdata]
# Img.step = Img.width
#
# Img.data = Img_data
# bag.write('camera/image_raw', Img, Stamp)
finally:
bag.close()
import time, sys, os
from ros import rosbag
import roslib, rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
TOPIC = 'camera/image'
bag = rosbag.Bag('test12345.bag', 'w')
cap = cv2.VideoCapture('/home/user/catkin_ws/src/tameshiwari/python/OCP_pendulum_2dof_rr/camera_image.jpeg')
cb = CvBridge()
prop_fps = 1
ret = True
frame_id = 0
while(ret):
ret, frame = cap.read()
if not ret:
break
stamp = rospy.rostime.Time.from_sec(float(frame_id) / prop_fps)
frame_id += 1
image = cb.cv2_to_imgmsg(frame, encoding='bgr8')
image.header.stamp = stamp
image.header.frame_id = "camera"
bag.write(TOPIC, image, stamp)
cap.release()
bag.close()