Data_input.py

#!/usr/bin/env python

################################################################

import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image,CompressedImage
from geometry_msgs.msg import Twist
import numpy as np
from look_caresses_pkg.msg import platform_config,platform_sensors,platform_state,platform_mics,platform_control,core_state,core_control,core_config,bridge_config,bridge_stream
import os
from os import listdir
from os.path import isfile, join
from sklearn.metrics import confusion_matrix
from pylab import *

import math
import numpy
import time
import sys
from miro_constants import miro
from keras.models import Sequential
from keras.layers import Dense, Dropout, Activation
from keras.layers.recurrent import LSTM
from keras.layers import Embedding
from keras.preprocessing.sequence import pad_sequences
from keras.utils import np_utils
from datetime import datetime
import keras
from keras import metrics
from keras import backend as K
from keras import regularizers
import tensorflow as tf

from keras.models import load_model


def fmt(x, f): #function used for conversion from byteArray to String (The values that we get for the head touch sensors are byteArrays)
    s = ""
    x = bytearray(x)
    for i in range(0, len(x)):
        if not i == 0:
            s = s + ", "
        s = s + f.format(x[i])
    return s



class miro_touch_control_ros_client:
    def callback_platform_sensors(self, subscriber_msg):
        # ignore until active
        if not self.active:
            return
        global touch_sensors    
        global new_data
        global dataset
        global send_data        
            global start_index
            global end_index
            global data_flag
            global pattern
            global count
            global model
            global graph
                
        
        #print(fmt(subscriber_msg.touch_head, '{0:.0f}')) #Uncomment and see what happens in output, It converts from byteArray to str
        
        new_data = True #set receive flag
        touch_sensors = [0,0,0,0,0,0,0,0]

        touch_sensors[0] = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[0]) # Sensor at 16'oclock on head
        touch_sensors[1] = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[3]) # Sensor at 14'oclock on head
        touch_sensors[2] = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[6]) # Sensor at 10'oclock on head
        touch_sensors[3] = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[9]) # Sensor at 20'oclock on head
        touch_sensors[4] = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[0]) # Sensor at 16'oclock on head
        touch_sensors[5] = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[3]) # Sensor at 14'oclock on head
        touch_sensors[6] = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[6]) # Sensor at 10'oclock on head
        touch_sensors[7] = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[9]) # Sensor at 20'oclock on head
        #print touch_sensors

    def loop(self):
        global new_data
        global touch_sensors
        global dataset
        global data_flag
        global start_index
        global end_index
        global pattern
        global send_data
        gesture_str = ""
        while not rospy.is_shutdown():
            if new_data:
                new_data = False #un-set flag
            
                #print aList
                dataset.append(touch_sensors)
                if len(dataset) == 105:
                    data_flag = True
                #print "before %d",len(dataset)
                if data_flag == True:
                    pattern = dataset
                    x_dataset = np.array(pattern)                                   
                        dataset.pop(0)
                    X_train = np.reshape(x_dataset, (1,x_dataset.shape[0], x_dataset.shape[1]))
                        XDataset = pad_sequences(X_train, maxlen=None, dtype='int32',padding='pre', truncating='pre', value=0.)
                        aux =0
                    with self.graph.as_default():
                        predict = self.model.predict(X_train)
                        threshold = 0.5
                        for i in range(len(predict[0])):
                            if predict[0][i] >= threshold:
                                aux = 1

                        if aux == 1:
                            index = np.argmax(predict) #index of the maximum value
                            if index == 0:
                                gesture_str = "CarBottomTop"
                            elif index == 1:
                                gesture_str = "FixedBody"
                            elif index == 2:
                                gesture_str = "PatHead"
                            elif index == 3:
                                gesture_str = "FixedHead"
                            elif index == 4:
                                gesture_str = "PatBody"
                            elif index == 5:
                                gesture_str = "CarTopBottom"
                        else:
                            gesture_str = "No_Gesture"

                        rospy.loginfo(gesture_str)                                     # It's like rosinfo in C++
                        self.pub.publish(gesture_str)

                        self.rate.sleep()


    def __init__(self):
        # report
        print("initialising...")
        print(sys.version)
        print(datetime.time(datetime.now()))

        # default data
        self.platform_sensors = None

        # options
        self.robot_name = ""
        
        # handle args
        for arg in sys.argv[1:]:
            f = arg.find('=')
            if f == -1:
                key = arg
                val = ""
            else:
                key = arg[:f]
                val = arg[f+1:]
            if key == "robot":
                self.robot_name = val
            else:
                error("argument not recognised \"" + arg + "\"")

            

        # check we got at least one
        if len(self.robot_name) == 0:
            error("argument \"robot\" must be specified")

        # set inactive
        self.active = False

        # topic root
        topic_root = "/miro/" + self.robot_name
        print "topic_root", topic_root

        # publish
        self.pub = rospy.Publisher('/miro/look4caresses/classifyGesture', String, queue_size=10)    # classifyGesture is the name of the topic on which we will publish , string is the parameter

        ####### Deleated some functions from here (compared to the orignal file)

        # subscribe
        self.sub_sensors = rospy.Subscriber(topic_root + "/platform/sensors", platform_sensors, self.callback_platform_sensors)
        ####### Deleated some functions from here (compared to the orignal file)
        global model
        global graph
        
        self.model = load_model('my_model_new.h5')
        self.model._make_predict_function()
        self.graph = tf.get_default_graph()

        adm = keras.optimizers.Adam(lr=0.00001, beta_1=0.9, beta_2=0.999, epsilon=None, decay=0.0, amsgrad=False)
        self.model.compile(loss='categorical_crossentropy',optimizer=adm ,metrics=['accuracy'])

        # set active
        self.active = True

        self.rate = rospy.Rate(20)

if __name__ == "__main__":
    global count
    global data_flag
    global dataset
    global pattern
    global send_data
    global start_index
        global end_index
        global model
        global graph
        global touch_sensors
        global new_data
        new_data = False
        touch_sensors = []
    start_index = 0
    end_index = 105
    pattern = []
    send_data = []
    dataset = []
    data_flag = False
    count = 0
    rospy.init_node("miro_touch_control_ros_client_py", anonymous=True)
    main = miro_touch_control_ros_client()
    main.loop()
    rospy.spin()