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()