Go to the documentation of this file.00001
00002
00003
00004
00005 import sys
00006 import numpy as np
00007 from threading import Condition
00008 import pickle
00009 import csv
00010
00011 import pygame
00012 from sklearn import preprocessing as pps, svm
00013
00014 import roslib; roslib.load_manifest('wouse')
00015 from Object import ROSObject
00016 import rospy
00017 from std_msgs.msg import Header
00018 from geometry_msgs.msg import Vector3Stamped
00019
00020 from wouse.srv import WouseRunStop, WouseRunStopRequest
00021
00022
00023 class Wouse(object):
00024 """
00025 Subscribes to mouse movements, detects wincing, and signals e-stop.
00026 """
00027 def __init__(self, svm_datafile):
00028 """Initialize svm classifier and needed services."""
00029 try:
00030 rospy.wait_for_service('wouse_run_stop', 5)
00031 self.runstop_client=rospy.ServiceProxy('wouse_run_stop',
00032 WouseRunStop, True)
00033 rospy.loginfo("Found wouse run-stop service")
00034 except:
00035 rospy.logerr("Cannot find wouse run-stop service")
00036 sys.exit()
00037
00038 (self.scaler, self.classifier) = self.init_classifier(svm_datafile)
00039 rospy.loginfo("SVM Trained on data from %s" %svm_datafile)
00040 rospy.Subscriber('wouse_movement', Vector3Stamped, self.movement_cb)
00041 rospy.Timer(rospy.Duration(0.1), self.ping_server)
00042 self.window = []
00043 self.data = []
00044 pygame.init()
00045 self.sound_new = pygame.mixer.Sound('../../sounds/new_item.wav')
00046
00047 rospy.loginfo("[%s]: Ready" %rospy.get_name())
00048
00049 def init_classifier(self, filename):
00050 """Unpickle svm training data, train classifier"""
00051 with open(filename, 'rb') as f:
00052 svm_data = pickle.load(f)
00053 labels = svm_data['labels']
00054 data = svm_data['data']
00055 scaler = pps.Scaler().fit(data)
00056 data_scaled = scaler.transform(data)
00057 classifier = svm.SVC()
00058 classifier.fit(data_scaled, labels)
00059 return (scaler, classifier)
00060
00061 def ping_server(self, event):
00062 """Send updated timestamp to Runstop server."""
00063 req = WouseRunStopRequest(False, False, rospy.Time.now())
00064 self.runstop_client(req)
00065
00066 def movement_cb(self, v3):
00067 """Filter out small movements, check classifier, call stop if needed."""
00068
00069
00070 if (v3.vector.x**2+v3.vector.y**2)**(0.5) < 2.5:
00071 return
00072 if self.classify_svm(v3.vector.x, v3.vector.y, v3.header.stamp):
00073 self.runstop_client(WouseRunStopRequest(True, False, rospy.Time.now()))
00074 self.sound_new.play()
00075 rospy.loginfo("Wince Detected, stopping robot!")
00076 else:
00077 rospy.loginfo("Not a wince")
00078
00079 def classify_svm(self, x, y, time):
00080 """Build the descriptor vector for the incoming mouse event, and classify with the svm."""
00081 datum = []
00082 mag = (x**2+y**2)**(0.5)
00083 angle = np.arctan2(y,x)
00084 datum.append(mag)
00085 datum.append(angle)
00086 self.window.append([x,y,time])
00087 while (self.window[-1][-1] - self.window[0][-1]) > rospy.Duration(0.25):
00088 self.window.pop(0)
00089 win = np.array(self.window)
00090 datum.append(len(self.window))
00091 win_x = np.sum(win[:,0])
00092 win_y = np.sum(win[:,1])
00093 win_mag = (win_x**2+win_y**2)**(0.5)
00094 datum.append(win_mag/len(self.window))
00095 datum.append(np.arctan2(win_y, win_x))
00096 datum_scaled = self.scaler.transform(datum)
00097 prediction = self.classifier.predict(datum_scaled)
00098 if prediction[0] == 1.:
00099 return True
00100
00101 if __name__=='__main__':
00102 rospy.init_node('wouse_node')
00103 wouse = Wouse(sys.argv[1])
00104 rospy.spin()