#!/usr/bin/env python


# Copyright (c) 2013-2014, Rethink Robotics
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
#    this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
# 3. Neither the name of the Rethink Robotics nor the names of its
#    contributors may be used to endorse or promote products derived from
#    this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

"""
copied from
Baxter RSDK Joint Position Example: file playback
"""

from __future__ import print_function


import sys

import rospy

import baxter_interface

from baxter_interface import CHECK_VERSION

import glob


from std_srvs.srv import Empty

def try_float(x):
    try:
        return float(x)
    except ValueError:
        return None


def clean_line(line, names):
    """
    Cleans a single line of recorded joint positions

    @param line: the line described in a list to process
    @param names: joint name keys
    """
    #convert the line of strings to a float or None
    line = [try_float(x) for x in line.rstrip().split(',')]
    #zip the values with the joint names
    combined = zip(names[1:], line[1:])
    #take out any tuples that have a none value
    cleaned = [x for x in combined if x[1] is not None]
    #convert it to a dictionary with only valid commands
    command = dict(cleaned)
    left_command = dict((key, command[key]) for key in command.keys()
                        if key[:-2] == 'left_')
    right_command = dict((key, command[key]) for key in command.keys()
                         if key[:-2] == 'right_')
    return (command, left_command, right_command, line)


def map_file(filename, loops=1):
    """
    Loops through csv file

    @param filename: the file to play
    @param loops: number of times to loop
                  values < 0 mean 'infinite'

    Does not loop indefinitely, but only until the file is read
    and processed. Reads each line, split up in columns and
    formats each line into a controller command in the form of
    name/value pairs. Names come from the column headers
    first column is the time stamp
    """
    left = baxter_interface.Limb('left')
    right = baxter_interface.Limb('right')
    grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
    grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
    rate = rospy.Rate(1000)

    if grip_left.error():
        grip_left.reset()
    if grip_right.error():
        grip_right.reset()
    if (not grip_left.calibrated() and
        grip_left.type() != 'custom'):
        grip_left.calibrate()
    if (not grip_right.calibrated() and
        grip_right.type() != 'custom'):
        grip_right.calibrate()

    print("Playing back: %s" % (filename,))
    with open(filename, 'r') as f:
        lines = f.readlines()
    keys = lines[0].rstrip().split(',')

    l = 0
    # If specified, repeat the file playback 'loops' number of times
    while loops < 1 or l < loops:
        i = 0
        l += 1
        print("Moving to start position...")

        _cmd, lcmd_start, rcmd_start, _raw = clean_line(lines[1], keys)
        left.move_to_joint_positions(lcmd_start)
        right.move_to_joint_positions(rcmd_start)
        start_time = rospy.get_time()
        for values in lines[1:]:
            i += 1
            loopstr = str(loops) if loops > 0 else "forever"
            sys.stdout.write("\r Record %d of %d, loop %d of %s" %
                             (i, len(lines) - 1, l, loopstr))
            sys.stdout.flush()

            cmd, lcmd, rcmd, values = clean_line(values, keys)
            #command this set of commands until the next frame
            while (rospy.get_time() - start_time) < values[0]:
                if rospy.is_shutdown():
                    print("\n Aborting - ROS shutdown")
                    return False
                if len(lcmd):
                    left.set_joint_positions(lcmd)
                if len(rcmd):
                    right.set_joint_positions(rcmd)
                if ('left_gripper' in cmd and
                    grip_left.type() != 'custom'):
                    grip_left.command_position(cmd['left_gripper'])
                if ('right_gripper' in cmd and
                    grip_right.type() != 'custom'):
                    grip_right.command_position(cmd['right_gripper'])
                rate.sleep()
        print
    return True

def main():
	dir = '/home/ros-baxter/sequence1/'
	fam = 'no'
	ext = '.rec'
	#fname = fam+'*'+ext
	#fam_list = glob.glob(ext)
	#print(fam_list)
	rospy.init_node("ferdian_file_playback")
	client = rospy.ServiceProxy("ferdian_example_service",Empty)
	rs = baxter_interface.RobotEnable(CHECK_VERSION)
	rs.enable()
	rospy.loginfo("waiting for service")
	rospy.wait_for_service("ferdian_example_service")
	rospy.loginfo("service available")
	#put your loop here
	for file in sorted(glob.glob('./sequence1/*.rec')):
		map_file(file)
		rospy.loginfo("sending signal...") # to the image processing node
	#for x in range(0, 3):
	#	map_file("AtoE.rec")
	res = client()
	rospy.loginfo("service returned")
    ###
    

if __name__ == '__main__':
    main()

[((95, 11, 95, 40), 'baxter_interface.Limb', 'baxter_interface.Limb', ({(95, 33, 95, 39): '"""left"""'}, {}), "('left')", False, 'import baxter_interface\n'), ((96, 12, 96, 42), 'baxter_interface.Limb', 'baxter_interface.Limb', ({(96, 34, 96, 41): '"""right"""'}, {}), "('right')", False, 'import baxter_interface\n'), ((97, 16, 97, 63), 'baxter_interface.Gripper', 'baxter_interface.Gripper', ({(97, 41, 97, 47): '"""left"""', (97, 49, 97, 62): 'CHECK_VERSION'}, {}), "('left', CHECK_VERSION)", False, 'import baxter_interface\n'), ((98, 17, 98, 65), 'baxter_interface.Gripper', 'baxter_interface.Gripper', ({(98, 42, 98, 49): '"""right"""', (98, 51, 98, 64): 'CHECK_VERSION'}, {}), "('right', CHECK_VERSION)", False, 'import baxter_interface\n'), ((99, 11, 99, 27), 'rospy.Rate', 'rospy.Rate', ({(99, 22, 99, 26): '1000'}, {}), '(1000)', False, 'import rospy\n'), ((162, 1, 162, 41), 'rospy.init_node', 'rospy.init_node', ({(162, 17, 162, 40): '"""ferdian_file_playback"""'}, {}), "('ferdian_file_playback')", False, 'import rospy\n'), ((163, 10, 163, 61), 'rospy.ServiceProxy', 'rospy.ServiceProxy', ({(163, 29, 163, 54): '"""ferdian_example_service"""', (163, 55, 163, 60): 'Empty'}, {}), "('ferdian_example_service', Empty)", False, 'import rospy\n'), ((164, 6, 164, 49), 'baxter_interface.RobotEnable', 'baxter_interface.RobotEnable', ({(164, 35, 164, 48): 'CHECK_VERSION'}, {}), '(CHECK_VERSION)', False, 'import baxter_interface\n'), ((166, 1, 166, 37), 'rospy.loginfo', 'rospy.loginfo', ({(166, 15, 166, 36): '"""waiting for service"""'}, {}), "('waiting for service')", False, 'import rospy\n'), ((167, 1, 167, 50), 'rospy.wait_for_service', 'rospy.wait_for_service', ({(167, 24, 167, 49): '"""ferdian_example_service"""'}, {}), "('ferdian_example_service')", False, 'import rospy\n'), ((168, 1, 168, 35), 'rospy.loginfo', 'rospy.loginfo', ({(168, 15, 168, 34): '"""service available"""'}, {}), "('service available')", False, 'import rospy\n'), ((176, 1, 176, 34), 'rospy.loginfo', 'rospy.loginfo', ({(176, 15, 176, 33): '"""service returned"""'}, {}), "('service returned')", False, 'import rospy\n'), ((127, 21, 127, 37), 'rospy.get_time', 'rospy.get_time', ({}, {}), '()', False, 'import rospy\n'), ((170, 20, 170, 50), 'glob.glob', 'glob.glob', ({(170, 30, 170, 49): '"""./sequence1/*.rec"""'}, {}), "('./sequence1/*.rec')", False, 'import glob\n'), ((172, 2, 172, 36), 'rospy.loginfo', 'rospy.loginfo', ({(172, 16, 172, 35): '"""sending signal..."""'}, {}), "('sending signal...')", False, 'import rospy\n'), ((133, 12, 133, 30), 'sys.stdout.flush', 'sys.stdout.flush', ({}, {}), '()', False, 'import sys\n'), ((138, 19, 138, 38), 'rospy.is_shutdown', 'rospy.is_shutdown', ({}, {}), '()', False, 'import rospy\n'), ((137, 19, 137, 35), 'rospy.get_time', 'rospy.get_time', ({}, {}), '()', False, 'import rospy\n')]