ordered_target_joint_states_merger.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('sr_utilities')
00003 import rospy
00004 from sensor_msgs.msg import JointState
00005 import thread
00006 
00007 
00008 class MergeMessages:
00009     def __init__(self):
00010         rospy.init_node('target_joint_state_merger', anonymous=True)
00011         self.subs_1 = rospy.Subscriber("/srh/target/joint_states", JointState, self.callback1)
00012         self.subs_2 = rospy.Subscriber("/sr_arm/target/joint_states", JointState, self.callback2)
00013         
00014         self.pub = rospy.Publisher("/targets/joint_states", JointState)
00015 
00016         self.msg_1_received = False
00017         self.msg_2_received = False
00018 
00019         self.joint_state_msg = JointState()
00020         
00021         self.mutex = thread.allocate_lock()
00022 
00023         rospy.spin()
00024 
00025     def callback1(self, data):
00026         self.mutex.acquire()
00027         if self.msg_1_received == True:
00028             self.mutex.release()
00029             return
00030 
00031         self.msg_1_received = True
00032 
00033         self.joint_state_msg.header.stamp = rospy.Time.now()
00034 
00035         tmp = data.name
00036         self.joint_state_msg.name = tmp
00037 
00038         tmp = data.position 
00039         self.joint_state_msg.position = tmp
00040 
00041         tmp = data.velocity 
00042         self.joint_state_msg.velocity = tmp
00043 
00044         tmp = data.effort
00045         self.joint_state_msg.effort = tmp
00046         
00047         self.mutex.release()
00048 
00049     def callback2(self, data):
00050         self.mutex.acquire()
00051 
00052         self.msg_2_received = True
00053 
00054         self.joint_state_msg.header.stamp = rospy.Time.now()
00055 
00056         tmp = self.joint_state_msg.name 
00057         tmp += data.name
00058         self.joint_state_msg.name = tmp
00059 
00060         tmp = self.joint_state_msg.position 
00061         tmp += data.position 
00062         self.joint_state_msg.position = tmp
00063 
00064         tmp = self.joint_state_msg.velocity 
00065         tmp += data.velocity 
00066         self.joint_state_msg.velocity = tmp
00067 
00068         tmp =  self.joint_state_msg.effort 
00069         tmp += data.effort
00070         self.joint_state_msg.effort = tmp
00071 
00072         if self.msg_1_received == True:
00073             self.pub.publish(self.joint_state_msg)
00074             self.msg_1_received = False
00075             self.msg_2_received = False
00076             self.joint_state_msg = JointState()
00077 
00078         self.mutex.release()
00079 
00080         
00081 
00082 if __name__ == '__main__':
00083     merger = MergeMessages()


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:44