import rosbag
import os
import sys

def main(argv):
        if len(argv) < 2:
                print "please give a bag path"
                sys.exit(1)
        bag_name = argv[1].split('/')[-1].split('.bag')[0]
        print (bag_name)
        path = argv[1].split(bag_name+'.bag')[0]
        left = [] 
        left_sys = []
        right =  []    
        right_sys = [] 
        middle = []
        middle_sys = [] 
        lidar = []
        final = []
        for topic, msg, t in rosbag.Bag(argv[1]).read_messages():
                if topic == "/usb_cam_left/image_raw":
                        # left.append(str(msg.header.stamp))
                        left.append("%.6f" % msg.header.stamp.to_sec())
                        left_sys.append("%.6f" % t.to_sec())
                        # print ("%.6f" % msg.header.stamp.to_sec())
                        # print ("%.6f" % t.to_sec())
                elif topic == "/usb_cam_right/image_raw":
                        right.append("%.6f" % msg.header.stamp.to_sec())
                        right_sys.append("%.6f" % t.to_sec())
                elif topic == "/usb_cam_middle/image_raw":
                        middle.append("%.6f" % msg.header.stamp.to_sec())   
                        middle_sys.append("%.6f" % t.to_sec())  
        # for topic, msg, t in rosbag.Bag(argv[2]).read_messages():
        #         if topic == "/pandar":
        #                 lidar.append("%.6f" % t.to_sec())
        # final = left
        # if len(right) != len(left):       
        #         if len(left) == len(middle):
        #                 final = left
        #         elif len(right) == len(middle):
        #                 final = right
        #         elif len(right) < len(right):
        #                 final = right
        #         else :
        #                 final = left
        with open("./" + bag_name + ".txt","w") as w :
                for index in range(0,len(left)):
                        w.write(str(left[index])+'\n')
                        # w.write(str(left_sys[index])+" "+str(middle_sys[index])+" "+str(right_sys[index])+" "+str(lidar[index])+'\n')
        w.close()
if __name__ == "__main__":
        main(sys.argv)