import rosbag
import os
import sys
import cv2
import numpy as np
import pypcd
import sensor_msgs.point_cloud2 as pc2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from cv_bridge import CvBridgeError


def main(argv):
        if len(argv) < 2:
                print "please give a bag path"
                sys.exit(1)
	#print path
	bag_name = argv[1].split('/')[-1].split('.')[0]
        path = argv[1].split('.bag')[0]
        if not os.path.exists(path):
                os.makedirs(path+"/pandar")
        print "****************begin***************"
        with rosbag.Bag(argv[1], 'r') as bag:
                for topic, msg, t in bag.read_messages():
                        if topic == "/pandar":
                                timestr = "%.6f" % msg.header.stamp.to_sec()
                                laser_data_name = timestr + ".pcd"
                                laser_data_path = os.path.join(path+"/pandar",laser_data_name)
                                pc = pypcd.PointCloud.from_msg(msg)
                                pc.save(laser_data_path)                
        bag.close()
        print "****************done****************"

if __name__ == "__main__":
        main(sys.argv)