$search
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