virtual_ShadowPST_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # This node combines 5 virtual touch sensors into a ShadowPST message compatible with etherCAT hand
00003 
00004 import roslib; roslib.load_manifest('sr_utilities')
00005 import rospy
00006 from std_msgs.msg import Float64,Int16
00007 from sr_robot_msgs.msg import ShadowPST
00008 import thread
00009 
00010 
00011 class MergeMessages:
00012     def __init__(self):
00013         rospy.init_node('ShadowPST_publisher', anonymous=True)
00014         self.ff_sub=rospy.Subscriber('/sr_tactile/touch/ff',Float64,self.ff_cb)
00015         self.mf_sub=rospy.Subscriber('/sr_tactile/touch/mf',Float64,self.mf_cb)
00016         self.rf_sub=rospy.Subscriber('/sr_tactile/touch/rf',Float64,self.rf_cb)
00017         self.lf_sub=rospy.Subscriber('/sr_tactile/touch/lf',Float64,self.lf_cb)
00018         self.th_sub=rospy.Subscriber('/sr_tactile/touch/th',Float64,self.th_cb)
00019         self.rate = rospy.Rate(25.0)
00020         self.pub = rospy.Publisher("/tactile", ShadowPST)
00021         self.pst=[0.0,0.0,0.0,0.0,0.0]
00022         self.mutex = thread.allocate_lock()
00023 
00024     def ff_cb(self, msg):
00025         self.mutex.acquire()
00026         self.pst[0]=msg.data
00027         self.mutex.release()
00028         
00029     def mf_cb(self, msg):
00030         self.mutex.acquire()
00031         self.pst[1]=msg.data
00032         self.mutex.release()
00033 
00034     def rf_cb(self, msg):
00035         self.mutex.acquire()
00036         self.pst[2]=msg.data
00037         self.mutex.release()
00038 
00039     def lf_cb(self, msg):
00040         self.mutex.acquire()
00041         self.pst[3]=msg.data
00042         self.mutex.release()
00043 
00044     def th_cb(self, msg):
00045         self.mutex.acquire()
00046         self.pst[4]=msg.data
00047         self.mutex.release()
00048         
00049     def shadowpst_publisher(self):  
00050         pst_state_msg=ShadowPST()
00051         pst_state_msg.temperature=[0,0,0,0,0,0]
00052         pressure=[]
00053         self.mutex.acquire()
00054         for i in range(0,5):
00055             pressure.append(self.pst[i]*100)
00056         pst_state_msg.pressure=pressure
00057         #print pst_state_msg.pressure
00058         self.mutex.release()
00059         
00060         pst_state_msg.header.stamp = rospy.Time.now()
00061         self.pub.publish(pst_state_msg)
00062 
00063 if __name__ == '__main__':
00064     merger = MergeMessages()
00065     while not rospy.is_shutdown():
00066         merger.shadowpst_publisher()
00067         merger.rate.sleep()
00068 


sr_tactile_sensors
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:59:56