#adaptation par bvandepo de http://answers.ros.org/question/11537/creating-a-bag-file-out-of-a-image-sequence/
#pour traiter des séquences d'images en niveaux de gris (pgm ou png) et les trier
import time, sys, os
from ros import rosbag
import roslib
import rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image

import ImageFile

def GetFilesFromDir(dir):
    '''Generates a list of files from the directory'''
    print( "Searching directory %s" % dir )
    all = []
    if os.path.exists(dir):
        for path, names, files in os.walk(dir):
            for f in files:
                if os.path.splitext(f)[1] in ['.png','.pgm']:
                    all.append( os.path.join( path, f ) )
    return all

def CreateMonoBag(imgs,bagname):
    '''Creates a bag file with camera images'''
    bag =rosbag.Bag(bagname, 'w')
#tri des images par nom croissant
    imgs=sorted(imgs)
    try:
        for i in range(len(imgs)):
            print("Adding %s" % imgs[i])
	    
            fp = open( imgs[i], "r" )
            p = ImageFile.Parser()

            while 1:
                s = fp.read(1024)
                if not s:
                    break
                p.feed(s)

            im = p.close()

            Stamp = rospy.rostime.Time.from_sec(time.time())
            Img = Image()
            Img.header.stamp = Stamp
            Img.width = im.size[0]
            Img.height = im.size[1]
            Img.encoding = "mono8"
            Img.header.frame_id = "camera"
            #Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
            Img_data = [pix for pix in im.getdata()] 
            Img.data = Img_data
            bag.write('camera/image_raw', Img, Stamp)
        
    finally:
        bag.close()       


def CreateBag(args):
    '''Creates the actual bag file by successively adding images'''
    all_imgs= GetFilesFromDir(args[0])
    if len(all_imgs) <= 0:
        print("No images found in %s" % args[0])
        exit()

    # create bagfile with mono camera image stream
    CreateMonoBag(all_imgs, args[1])        

if __name__ == "__main__":
    if len( sys.argv ) == 3:
        CreateBag( sys.argv[1:])
    else:
        print( "Usage: img2bag_gray_mono imagedir bagfilename")