tactile_sensors.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 import roslib
00003 roslib.load_manifest('rfid_behaviors')
00004 roslib.load_manifest('pr2_msgs')
00005 roslib.load_manifest('std_msgs')
00006 import rospy
00007 
00008 from pr2_msgs.msg import PressureState
00009 from std_msgs.msg import Float64
00010 from rfid_behaviors.srv import FloatFloat_Int32
00011 
00012 import numpy as np, math
00013 import time, string
00014 
00015 def default_mag_func( x ):
00016     return np.sum( np.abs( x ))
00017 
00018 class TactileSensor( ):
00019     def __init__( self, topic = '/pressure/r_gripper_motor', 
00020                   mag_func = default_mag_func ):
00021         rospy.logout( 'tactile_sensor: Initializing' )
00022         try:
00023             rospy.init_node( 'tactile_sensor' )
00024         except:
00025             pass
00026 
00027         self.mag_func = mag_func
00028         self.left = None
00029         self.right = None
00030         self.l_bias = None
00031         self.r_bias = None
00032         self.topic = topic
00033 
00034         self.pub = rospy.Publisher( '/readings/' + string.replace(topic,'/','_') + '_mag', Float64 )
00035         self.service = rospy.Service( '/readings/' + string.replace(topic,'/','_') + '_serv', 
00036                                       FloatFloat_Int32, 
00037                                       self.thresh_service )
00038         self.reg_sensor( )
00039 
00040         while self.left == None or self.right == None:
00041             time.sleep( 0.1 )
00042 
00043         self.unreg_sensor()
00044 
00045         rospy.logout( 'tactile_sensor: Ready' )
00046 
00047 
00048     def reg_sensor( self ):
00049         self.sub = rospy.Subscriber( self.topic, PressureState, self.cb )
00050         time.sleep( 0.3 )
00051 
00052     def unreg_sensor( self ):
00053         self.sub.unregister()
00054 
00055 
00056     def cb( self, msg ):
00057         self.left = np.array( list( msg.l_finger_tip ), dtype=float )
00058         self.right = np.array( list( msg.r_finger_tip ), dtype=float )
00059 
00060         if self.l_bias == None or self.r_bias == None:
00061             self.l_bias = np.zeros( len( self.left ))
00062             self.r_bias = np.zeros( len( self.right ))
00063 
00064         self.l_read = np.copy( self.left - self.l_bias )
00065         self.r_read = np.copy( self.right - self.r_bias )
00066 
00067         self.mag = self.mag_func( np.append( self.l_read, self.r_read ))
00068         #print np.append( self.l_read, self.r_read )
00069         self.pub.publish( self.mag )
00070         
00071 
00072     def bias( self ):
00073         self.reg_sensor()
00074         rospy.logout( 'tactile_sensor: Biasing' )
00075         self.l_bias = np.copy( self.left )
00076         self.r_bias = np.copy( self.right )
00077         self.unreg_sensor()
00078         return True
00079 
00080     def read( self ):
00081         return np.copy( self.l_read ), np.copy( self.r_read )
00082 
00083     def thresh_service( self, request ):
00084         self.thresh_detect( request.rotate, request.displace ) # yeah, jacked up names
00085         return int( True )
00086 
00087     def thresh_detect( self, threshold, timeout = 100.0 ):
00088         rospy.logout( 'tactile_sensor: Threshold detector activated: %3.2f, timeout: %d' % (threshold, timeout))
00089         self.bias()
00090         self.reg_sensor()
00091         t0 = time.time()
00092         t_diff = time.time() - t0
00093         lp = t0 # Printing status messages
00094         while self.mag < threshold and t_diff < timeout:
00095             if time.time() - lp > 1.0:
00096                 lp = time.time()
00097                 rospy.logout( 'tactile_sensor: Threshold still undetected' )
00098             time.sleep( 0.05 )
00099             t_diff = time.time() - t0
00100         self.unreg_sensor()
00101         rospy.logout( 'tactile_sensor: Detected (or timeout)' )
00102         return self.mag
00103 
00104 if __name__ == '__main__':
00105     import optparse
00106     p = optparse.OptionParser()
00107     p.add_option('-r', '--right', action='store_true', dest='right', default=False,
00108                  help='Right finger')
00109     p.add_option('-l', '--left', action='store_true', dest='left', default=False,
00110                  help='left finger')
00111     opt, args = p.parse_args()
00112     
00113     if opt.right:
00114         r_tact = TactileSensor('/pressure/r_gripper_motor')
00115     if opt.left:
00116         l_tact = TactileSensor('/pressure/l_gripper_motor')
00117 
00118     rospy.spin()
00119 
00120 
00121 
00122 #     print 'Reading: ', r_tact.read()
00123 
00124 #     raw_input('Hit [ENTER]')
00125     
00126 #     r_tact.bias()
00127 #     time.sleep(3.0)
00128 #     print 'Reading: ', r_tact.read()
00129     
00130 #     raw_input('Hit [ENTER]')
00131 
00132 #     print 'COMBINED:', np.append( *r_tact.read() )
00133 #     print 'ERR:', np.sum(np.abs( np.append( *r_tact.read() )))
00134 
00135 #     print 'Waiting for thresh'
00136 #     r_tact.thresh_detect( 5000 )


rfid_behaviors
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 11:50:15