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