00001 import roslib; roslib.load_manifest('hrl_pr2_lib') 00002 import rospy 00003 import numpy as np 00004 import pr2_msgs.msg as pm 00005 00006 00007 class PressureListener: 00008 def __init__(self, topic='/pressure/l_gripper_motor', safe_pressure_threshold = 4000): 00009 rospy.Subscriber(topic, pm.PressureState, self.press_cb) 00010 self.lmat = None 00011 self.rmat = None 00012 self.lmat0 = None 00013 self.rmat0 = None 00014 self.lmat_raw = None 00015 self.rmat_raw = None 00016 00017 self.safe_pressure_threshold = safe_pressure_threshold 00018 self.exceeded_safe_threshold = False 00019 00020 self.threshold = None 00021 self.exceeded_threshold = False 00022 00023 def rezero(self): 00024 self.lmat0 = None 00025 self.rmat0 = None 00026 self.exceeded_threshold = False 00027 self.exceeded_safe_threshold = False 00028 00029 def check_safety_threshold(self): 00030 r = self.exceeded_safe_threshold 00031 self.exceeded_safe_threshold = False #reset 00032 return r 00033 00034 def check_threshold(self): 00035 r = self.exceeded_threshold 00036 self.exceeded_threshold = False 00037 return r 00038 00039 def set_threshold(self, threshold): 00040 self.threshold = threshold 00041 00042 def get_pressure_readings(self): 00043 return self.lmat, self.rmat 00044 00045 def press_cb(self, pmsg): 00046 lmat = np.matrix((pmsg.l_finger_tip)).T 00047 rmat = np.matrix((pmsg.r_finger_tip)).T 00048 if self.lmat0 == None: 00049 self.lmat0 = lmat 00050 self.rmat0 = rmat 00051 return 00052 00053 self.lmat_raw = lmat 00054 self.rmat_raw = rmat 00055 00056 lmat = lmat - self.lmat0 00057 rmat = rmat - self.rmat0 00058 00059 #touch detected 00060 if np.any(np.abs(lmat) > self.safe_pressure_threshold) or np.any(np.abs(rmat) > self.safe_pressure_threshold): 00061 self.exceeded_safe_threshold = True 00062 00063 if self.threshold != None and (np.any(np.abs(lmat) > self.threshold) or np.any(np.abs(rmat) > self.threshold)): 00064 #print 'EXCEEDED threshold', self.threshold 00065 #print 'PressureListener: ', np.max(np.abs(lmat)), np.max(np.abs(rmat)), 'threshold', self.threshold 00066 self.exceeded_threshold = True 00067 00068 self.lmat = lmat 00069 self.rmat = rmat 00070 00071