00001
00002 import roslib; roslib.load_manifest('cob_tactiletools')
00003 import rospy
00004 from cob_sdh.msg import TactileMatrix, TactileSensor
00005 from cob_srvs.srv import *
00006 from std_msgs.msg import Bool, Float32MultiArray
00007
00008
00009
00010 class TactileFilters():
00011 def __init__(self):
00012 mean_value_publisher = 0
00013 grabbed_publisher = 0
00014 cylindric_grabbed_publisher = 0
00015 touched_treshold = 0
00016 self.is_grasped = False
00017 self.is_cylindric_grasped = False
00018 self.one_pad_contact = False
00019
00020 def getMean(self, tarray):
00021 sum = 0
00022 for i in tarray:
00023 sum += i
00024 return sum/len(tarray)
00025
00026
00027 def roscb(self, data):
00028 matrices = data.tactile_matrix
00029 meanvalues = Float32MultiArray()
00030 touched = 0
00031 touched_finger = 0
00032 touched_thumb = 0
00033 meanvalues.data = []
00034 for m in matrices:
00035 meanvalues.data.insert(m.matrix_id, self.getMean(m.tactile_array))
00036 if(meanvalues.data[m.matrix_id] > self.touched_treshold):
00037 touched += 1
00038 if((m.matrix_id == 2) or (m.matrix_id == 3)):
00039 touched_thumb += 1
00040 else:
00041 touched_finger += 1
00042 self.mean_value_publisher.publish(meanvalues)
00043 if(touched >= 2):
00044 self.is_grasped = True
00045 self.grabbed_publisher.publish(Bool(True))
00046 else:
00047 self.is_grasped = False
00048 self.grabbed_publisher.publish(Bool(False))
00049 if((touched_thumb >= 1) and (touched_finger >= 1)):
00050 self.is_cylindric_grasped = True
00051 self.cylindric_grabbed_publisher.publish(Bool(True))
00052 else:
00053 self.is_cylindric_grasped = False
00054 self.cylindric_grabbed_publisher.publish(Bool(False))
00055 if(touched >= 1):
00056 self.one_pad_contact = True
00057 self.one_pad_contact_publisher.publish(Bool(True))
00058 else:
00059 self.one_pad_contact = False
00060 self.one_pad_contact_publisher.publish(Bool(False))
00061
00062 def handle_is_grasped(self, req):
00063 res = TriggerResponse()
00064 if self.is_grasped == True:
00065 res.success.data = True
00066 res.error_message.data = "grasped object"
00067 else:
00068 res.success.data = False
00069 res.error_message.data= "object not grasped"
00070
00071 return res
00072
00073 def handle_is_cylindric_grasped(self, req):
00074 res = TriggerResponse()
00075 if self.is_cylindric_grasped == True:
00076 res.success.data = True
00077 res.error_message.data = "grasped object"
00078 else:
00079 res.success.data = False
00080 res.error_message.data= "object not grasped"
00081
00082 return res
00083
00084 def handle_one_pad_contact(self, req):
00085 res = TriggerResponse()
00086 res.success.data = self.one_pad_contact
00087 return res
00088
00089 if (__name__ == "__main__"):
00090 TF = TactileFilters()
00091 rospy.init_node('TactileSensorView', anonymous=True)
00092 rospy.Subscriber("/sdh_controller/tactile_data", TactileSensor, TF.roscb)
00093 TF.mean_value_publisher = rospy.Publisher("/sdh_controller/mean_values", Float32MultiArray)
00094 TF.one_pad_contact_publisher = rospy.Publisher("/sdh_controller/one_pad_contact", Bool)
00095 TF.grabbed_publisher = rospy.Publisher("/sdh_controller/grabbed", Bool)
00096 service_is_grasped = rospy.Service('/sdh_controller/is_grasped', Trigger, TF.handle_is_grasped)
00097 TF.cylindric_grabbed_publisher = rospy.Publisher("/sdh_controller/cylindric_grabbed", Bool)
00098 service_is_cylindric_grasped = rospy.Service('/sdh_controller/is_cylindric_grasped', Trigger, TF.handle_is_cylindric_grasped)
00099 service_one_pad_contact = rospy.Service('/sdh_controller/one_pad_contact', Trigger, TF.handle_one_pad_contact)
00100 treshold = rospy.get_param('TouchedTreshold', '10')
00101 TF.touched_treshold = treshold
00102 print "Setting touched treshold to ", treshold
00103 while not rospy.is_shutdown():
00104 rospy.sleep(1.0)
00105
