Code: JoyFace integrated with V-Rep simulation


In [ ]:
#%matplotlib inline
import time
import cv2
import matplotlib.pyplot as plt
from IPython import display

In [ ]:
##All the functions

def set_classifier(cascPath):
    cascPath = cascPath
    faceCascade = cv2.CascadeClassifier(cascPath)
    return faceCascade
 
def set_nose_point(x,y,w,h,frame):
    #Setting the nose point
    face_x = (x+(w/2))
    face_y = (y+(h/2))
    #Show the green nose point
    cv2.circle(frame,(face_x,face_y), 8, (0,255,0),-1)
           
def set_direction(x,y,ax,ay,stop):
    if(stop == True):
        return "STOP"
    else:
        if y < ay-precision_v:
            cv2.rectangle(frame, (average_x-precision_h,average_y-precision_v),
                                     (x4,y4), (0,255,0),-1)
            return "UP"
        elif x < ax-precision_h:
            cv2.rectangle(frame, (average_x-precision_h,average_y-precision_v),
                                         (x3,y3), (0,255,0),-1)
            return "LEFT"
        elif x > ax+precision_h:
            cv2.rectangle(frame, (x5,y5),(x6,y6), (0,255,0),-1)
            return "RIGHT"

In [ ]:
#Configuration Variables: don't should be alterated
calc_average = True
count_face = 0 
average_x = 0 
average_y = 0 
sp=0

#This variables below can be alterated

#Change the size of the threshold area
precision_h = 20
precision_v = 15

#For security iniciate the wheelchair with the command STOP
send_command = "STOP"

#Set the velocity of the wheelchair
velocity = 0.5

In [ ]:
try:
    import vrep
except:
    print ('"vrep.py" could not be imported."') 

print ('Program started')
#just in case, close all opened connections
vrep.simxFinish(-1) 
#Connect to V-REP
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) 

if clientID!=-1:
    print ('Connected to remote API server')
    vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
    res,leftHandle=vrep.simxGetObjectHandle(clientID,'leftM',vrep.simx_opmode_blocking)
    res,rightHandle=vrep.simxGetObjectHandle(clientID,'rightM',vrep.simx_opmode_blocking)

In [ ]:
#Open the first webcam
vc = cv2.VideoCapture(0)
# try to get the first frame
if vc.isOpened(): 
    is_capturing, frame = vc.read()  
    webcam_preview = plt.imshow(frame)    
else:
    is_capturing = False

while is_capturing:
    try:    
        #Iniciate to read the webcam
        is_capturing, frame = vc.read()
        #Flip the image
        frame = cv2.flip(frame,180) 
        #Filter grayscale
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        #Show for the user the image with colors
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)  
        #Set the OpenCV Face Classifier
        faceCascade = set_classifier('haarcascade_frontalface_default.xml')
        #Set the OpenCV Smile Classifier
        smileCascade = set_classifier('haarcascade_smile.xml')

        #Set the parameters for detect the face
        faces = faceCascade.detectMultiScale(
            gray,
            scaleFactor=1.1,
            minNeighbors=8,
            minSize=(120, 120)
        )
        #Found the face
        for (x, y, w, h) in faces:
            #Draw the green rectangle of the face
            #cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
            
            #Calcule the centroid of the face and set green point
            set_nose_point(x,y,w,h,frame)    
            
            #Define Region of Intereting of Smile
            roi_smile = frame[y:y+h, x:x+w]
            
            #Set the parameters for detect the smile
            smile = smileCascade.detectMultiScale(
            roi_smile,
            scaleFactor=2.7,
            minNeighbors=20,
            minSize=(10, 10)  
            )
            
            #Found the smile
            for(sp,sq,sr,ss) in smile:
                #Draw the red rectangle of the smile
                cv2.rectangle(roi_smile,(sp,sq),(sp+sr,sq+ss), (255,0,0),thickness=2)
                
            #Any time the smile can stop the wheelchair
            if sp in smile:
                stop = True
            else:
                stop = False

            #Calcule the threshold white area
            if calc_average:
                count_face += 1
                average_x += (x+(w/2))
                average_y += (y+(h/2))
                progress = (count_face/60.0) * 100
                
                if count_face==3:
                    average_x /= count_face
                    average_y /= count_face
                    calc_average = False
            else:   
                face_x = (x+(w/2)) 
                face_y = (y+(h/2))
                flag_save = True
                
                #Draw the threshold white area
                cv2.rectangle(frame, (average_x-precision_h,average_y-precision_v),
                                         (average_x+precision_h,average_y+precision_v), (255,255,255),thickness=2)   
                
                x3 = ((average_x-precision_h) - ((average_x+precision_h)-(average_x-precision_h)))
                y3 = average_y+precision_v
                x4 = ((average_x-precision_h) + ((average_x+precision_h)-(average_x-precision_h)))
                y4 = ((average_y+precision_v) + ((average_y-precision_v)-(average_y+precision_v)))            
                y4 = y4 +((average_y-precision_v)-(average_y+precision_v))
                x5 = average_x-precision_h + ((average_x+precision_h)-(average_x-precision_h))
                y5 = average_y-precision_v
                x6 = x5 + ((average_x+precision_h)-(average_x-precision_h))
                y6 = y5-((average_y-precision_v)-(average_y+precision_v))
                x7 = x3 + ((average_x+precision_h)-(average_x-precision_h))
                y7 = average_y+precision_v
                x8 = x7 + ((average_x+precision_h)-(average_x-precision_h))
                y8 = y7 - ((average_y-precision_v)-(average_y+precision_v))


                #Quadrado Esquerda
                cv2.rectangle(frame, (average_x-precision_h,average_y-precision_v),
                                         (x3,y3), (255,255,255),thickness=2)
                #put_msg("<",frame,((average_x-precision_h) +(average_x-precision_h)/2),y3+(y3/2))
                center_esquerdaX = (average_x-precision_h-(x3 - average_x-precision_h))
                center_esquerdaY = average_y-precision_v-(y3-average_y-precision_v)

                #Quadrado acima

                cv2.rectangle(frame, (average_x-precision_h,average_y-precision_v),
                                         (x4,y4), (255,255,255),thickness=2)

                #Quadrado Direita
                cv2.rectangle(frame, (x5,y5),(x6,y6), (255,255,255),thickness=2)

                #Verify the direction of the face and associate with a command: UP,LEFT,RIGHT,STOP
                send_command = set_direction(face_x,face_y,average_x,average_y,stop)
                
                #Send the commands fot the wheelchair
                if(send_command == "UP"):
                    vrep.simxSetJointTargetVelocity(clientID,leftHandle,velocity,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointTargetVelocity(clientID,rightHandle,velocity,vrep.simx_opmode_oneshot)
                if(send_command == "RIGHT"):
                    vrep.simxSetJointTargetVelocity(clientID,leftHandle,velocity,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointTargetVelocity(clientID,rightHandle,-velocity,vrep.simx_opmode_oneshot)
                    time.sleep(0.5)
                    vrep.simxSetJointTargetVelocity(clientID,leftHandle,velocity,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointTargetVelocity(clientID,rightHandle,velocity,vrep.simx_opmode_oneshot)
                if(send_command == "LEFT"):
                    vrep.simxSetJointTargetVelocity(clientID,rightHandle,velocity,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointTargetVelocity(clientID,leftHandle,-velocity,vrep.simx_opmode_oneshot)
                    time.sleep(0.5)
                    vrep.simxSetJointTargetVelocity(clientID,leftHandle,velocity,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointTargetVelocity(clientID,rightHandle,velocity,vrep.simx_opmode_oneshot)
                if(send_command == "STOP"):
                    vrep.simxSetJointTargetVelocity(clientID,rightHandle,0,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointTargetVelocity(clientID,leftHandle,0,vrep.simx_opmode_oneshot)
        
        #See the webcam feedback                                    
        webcam_preview.set_data(frame)
        plt.draw()

        display.clear_output(wait=True)
        display.display(plt.gcf())

    except KeyboardInterrupt:
        vc.release()