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