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