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