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('position_joint_state_merger', anonymous=True) 00011 self.subs_1 = rospy.Subscriber("/srh/position/joint_states", JointState, self.callback1) 00012 self.subs_2 = rospy.Subscriber("/sr_arm/position/joint_states", JointState, self.callback2) 00013 00014 self.pub = rospy.Publisher("/positions/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 self.mutex.release() 00047 00048 def callback2(self, data): 00049 self.mutex.acquire() 00050 00051 self.msg_2_received = True 00052 00053 self.joint_state_msg.header.stamp = rospy.Time.now() 00054 00055 tmp = self.joint_state_msg.name 00056 tmp += data.name 00057 self.joint_state_msg.name = tmp 00058 00059 tmp = self.joint_state_msg.position 00060 tmp += data.position 00061 self.joint_state_msg.position = tmp 00062 00063 tmp = self.joint_state_msg.velocity 00064 tmp += data.velocity 00065 self.joint_state_msg.velocity = tmp 00066 00067 tmp = self.joint_state_msg.effort 00068 tmp += data.effort 00069 self.joint_state_msg.effort = tmp 00070 00071 if self.msg_1_received == True: 00072 self.pub.publish(self.joint_state_msg) 00073 self.msg_1_received = False 00074 self.msg_2_received = False 00075 self.joint_state_msg = JointState() 00076 00077 self.mutex.release() 00078 00079 00080 00081 if __name__ == '__main__': 00082 merger = MergeMessages()